def createInertials(link, empty=False, preserve_children=False): """Creates inertial representations for visual and collision objects in link. :param link: The link you want to create the inertial for. :type link: bpy_types.Object :param empty: If set to True the new inertial object will contain no information. :type empty: bool :param preserve_children: If set to False already existent inertial objects will be deleted. :type preserve_children: bool """ # viscols = getInertiaRelevantObjects(link) # clean existing data if not preserve_children: oldinertials = selectionUtils.getImmediateChildren(link, ['inertial']) else: try: oldinertials = [bpy.data.objects['inertial_' + link.name]] except KeyError: oldinertials = None if oldinertials: selectionUtils.selectObjects(oldinertials, clear=True, active=0) bpy.ops.object.delete() if not preserve_children: for obj in viscols: if not empty: mass = obj['mass'] if 'mass' in obj else None geometry = robotdictionary.deriveGeometry(obj) if mass is not None: if geometry['type'] == 'mesh': selectionUtils.selectObjects([obj]) bpy.context.scene.objects.active = obj inert = calculateMeshInertia(obj.data, mass) #print('mesh:', inert) #print('ellipsoid:', calculateEllipsoidInertia(mass, geometry['size'])) #print('box:', calculateBoxInertia(mass, geometry['size'])) else: inert = calculateInertia(mass, geometry) if inert is not None: inertial = createInertial(obj) inertial['mass'] = mass inertial['inertia'] = inert else: createInertial(obj) # compose inertial object for link if not empty: mass, com, inert = fuseInertiaData( selectionUtils.getImmediateChildren(link, ['inertial'])) if mass and com and inert: inertial = createInertial(link) com_translate = mathutils.Matrix.Translation(com) inertial.matrix_local = com_translate bpy.ops.transform.translate(value=( 0, 0, 0 )) # FIXME: this is a trick to force Blender to apply matrix_local inertial['inertial/mass'] = mass inertial['inertial/inertia'] = inertiaMatrixToList(inert) else: createInertial(link)
def getInertiaChildren(link, selected_only=False, include_hidden=False): """Returns a list of the inertia objects which are children of a link. :param link: The link you want to gather the inertia relevant objects for :type link: bpy.types.Object :param selected_only: return only relevant objects which are selected :type selected_only: bool :param include_hidden: include hidden inertia objects :type include_hidden: bool :return: list of child inertial objects of the link :rtype: list """ assert link.phobostype == 'link', "Object is not a link." assert isinstance(selected_only, bool), "Not a boolean: " + type(selected_only) assert isinstance(include_hidden, bool), "Not a boolean: " + type(include_hidden) # Get the inertia objects inertiaobjects = sUtils.getImmediateChildren(link, ('inertial'), selected_only, include_hidden) # Get the visuals and collisions viscols = getInertiaRelevantObjects(link, selected_only) # Iterate over the objects and get the inertia objects for obj in viscols: # Add inertia objects to the list inertiaobjects += sUtils.getImmediateChildren(obj, ('inertial'), selected_only, include_hidden) return inertiaobjects
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 createInertials(link, empty=False, preserve_children=False): """Creates inertial representations for visual and collision objects in link. :param link: The link you want to create the inertial for. :type link: bpy_types.Object :param empty: If set to True the new inertial object will contain no information. :type empty: bool :param preserve_children: If set to False already existent inertial objects will be deleted. :type preserve_children: bool """ # viscols = getInertiaRelevantObjects(link) # clean existing data if not preserve_children: oldinertials = selectionUtils.getImmediateChildren(link, ['inertial']) else: try: oldinertials = [bpy.data.objects['inertial_'+link.name]] except KeyError: oldinertials = None if oldinertials: selectionUtils.selectObjects(oldinertials, clear=True, active=0) bpy.ops.object.delete() if not preserve_children: for obj in viscols: if not empty: mass = obj['mass'] if 'mass' in obj else None geometry = robotdictionary.deriveGeometry(obj) if mass is not None: if geometry['type'] == 'mesh': selectionUtils.selectObjects([obj]) bpy.context.scene.objects.active = obj inert = calculateMeshInertia(obj.data, mass) #print('mesh:', inert) #print('ellipsoid:', calculateEllipsoidInertia(mass, geometry['size'])) #print('box:', calculateBoxInertia(mass, geometry['size'])) else: inert = calculateInertia(mass, geometry) if inert is not None: inertial = createInertial(obj) inertial['mass'] = mass inertial['inertia'] = inert else: createInertial(obj) # compose inertial object for link if not empty: mass, com, inert = fuseInertiaData(selectionUtils.getImmediateChildren(link, ['inertial'])) if mass and com and inert: inertial = createInertial(link) com_translate = mathutils.Matrix.Translation(com) inertial.matrix_local = com_translate bpy.ops.transform.translate(value=(0, 0, 0)) # FIXME: this is a trick to force Blender to apply matrix_local inertial['inertial/mass'] = mass inertial['inertial/inertia'] = inertiaMatrixToList(inert) else: createInertial(link)
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 createLinkInertialObjects(link, autocalc=True, selected_only=False): """Creates inertial representations from helper inertials of a link. The new link inertial can contain automatically calculated inertia or remain empty (based on the autocalc parameter). If the selected_only parameter is used the inertia is calculated including only the selected helper inertials objects. :param link: The link you want to create the inertial for. :type link: bpy_types.Object :param autocalc: If set to False the new inertial object will contain no inertia information. :type autocalc: bool. :param selected_only: If set to True the inertia calculation uses only the currently selected inertial objects. :type selected_only: bool. """ inertias = sUtils.getImmediateChildren(link, ('inertial', ), selected_only, True) inertialdata = { 'mass': 0, 'inertia': (0, 0, 0, 0, 0, 0), 'pose': { 'translation': mathutils.Vector((0, 0, 0)) } } # compose inertial object for link from helper inertials if autocalc: mass, com, inert = fuseInertiaData(inertias) if mass and com and inert: inertialdata['mass'] = mass inertialdata['inertia'] = inertiaMatrixToList(inert) inertialdata['pose'] = {'translation': com} # create empty inertial object createInertial(link.name, inertialdata, parentobj=link)
def deriveLinkfromObject(obj, scale=0.2, parent_link=True, parent_objects=False, nameformat=''): """Derives a link from an object using its name, transformation and parenting. Args: obj(bpy_types.Object): object to derive a link from scale(float, optional): scale factor for bone size (Default value = 0.2) parent_link(bool, optional): whether to automate the parenting of the new link or not. (Default value = True) parent_objects(bool, optional): whether to parent all the objects to the new link or not (Default value = False) nameformat(str, optional): re-formatting template for obj names (Default value = '') Returns: : newly created link """ log('Deriving link from ' + nUtils.getObjectName(obj), level="INFO") try: nameparts = [p for p in re.split('[^a-zA-Z]', nUtils.getObjectName(obj)) if p != ''] linkname = nameformat.format(*nameparts) except IndexError: log('Invalid name format (indices) for naming: ' + nUtils.getObjectName(obj), 'WARNING') linkname = 'link_' + nUtils.getObjectName(obj) link = createLink({'scale': scale, 'name': linkname, 'matrix': obj.matrix_world}) # parent link to object's parent if parent_link: if obj.parent: eUtils.parentObjectsTo(link, obj.parent) # parent children of object to link if parent_objects: children = [obj] + sUtils.getImmediateChildren(obj) eUtils.parentObjectsTo(children, link, clear=True) return link
def deriveEntity(light, outpath): """This function handles a light entity in a scene to export it Args: entity(bpy.types.Object): The lights root object. outpath(str): The path to export to. Not used for light entity savetosubfolder(bool): If True data will be exported into subfolders. Not used for light entity light: Returns: : dict - An entry for the scenes entitiesList """ log("Exporting " + light["entity/name"] + " as a light entity", "INFO") entitypose = models.deriveObjectPose(light) lightobj = sUtils.getImmediateChildren(light)[0] color = lightobj.data.color entity = { "name": light["entity/name"], "type": "light", "light_type": "spotlight" if lightobj.data.type == "SPOT" else "omnilight", "anchor": light["anchor"] if "anchor" in light else "none", "color": { "diffuse": [color.r, color.g, color.b], "use_specular": lightobj.data. use_specular, # only specular information currently available }, "position": entitypose["translation"], "rotation": entitypose["rotation_quaternion"], } if entity["light_type"] == "spotlight": entity["angle"] = lightobj.data.spot_size return entity
def deriveEntity(light, outpath): """This function handles a light entity in a scene to export it Args: entity(bpy.types.Object): The lights root object. outpath(str): The path to export to. Not used for light entity savetosubfolder(bool): If True data will be exported into subfolders. Not used for light entity light: Returns: : dict - An entry for the scenes entitiesList """ log("Exporting " + light["entity/name"] + " as a light entity", "INFO") entitypose = models.deriveObjectPose(light) lightobj = sUtils.getImmediateChildren(light)[0] color = lightobj.data.color entity = { "name": light["entity/name"], "type": "light", "light_type": "spotlight" if lightobj.data.type == "SPOT" else "omnilight", "anchor": light["anchor"] if "anchor" in light else "none", "color": { "diffuse": [color.r, color.g, color.b], "use_specular": lightobj.data.use_specular, # only specular information currently available }, "position": entitypose["translation"], "rotation": entitypose["rotation_quaternion"], } if entity["light_type"] == "spotlight": entity["angle"] = lightobj.data.spot_size return entity
def deriveLinkfromObject(obj, scale=0.2, parenting=True, parentobjects=False, namepartindices=[], separator='_', prefix='link'): """Derives a link from an object that defines a joint through its position, orientation and parent-child relationships. :param obj: The object you want to derive your link from. :type obj: bpy_types.Object :param scale: The scale you want to apply to the link. :type scale: float :param parenting: Whether you want to automate the parenting of the new link or not. :type parenting: bool. :param parentobjects: Whether you want to parent all the objects to the new link or not. :type parentobjects: bool. :param namepartindices: Parts of the objects name you want to reuse in the links name. :type namepartindices: list with two elements. :param separator: The separator you want to use to separate the links name with. Its '_' per default :type separator: str :param prefix: The prefix you want to use for the new links name. Its 'link' per default. :type prefix: str """ print('Deriving link from', namingUtils.getObjectName(obj)) nameparts = namingUtils.getObjectName(obj).split('_') rotation = obj.matrix_world.to_euler() if 'invertAxis' in obj and obj['invertAxis'] == 1: rotation.x += math.pi if rotation.x < 0 else -math.pi tmpname = namingUtils.getObjectName(obj) if namepartindices: try: tmpname = separator.join([nameparts[p] for p in namepartindices]) except IndexError: print('Wrong name segment indices given for obj', namingUtils.getObjectName(obj)) if prefix != '': tmpname = prefix + separator + tmpname if tmpname == namingUtils.getObjectName(obj): obj.name += '*' link = createLink(scale, obj.matrix_world.to_translation(), obj.matrix_world.to_euler(), tmpname) if parenting: if obj.parent: selectionUtils.selectObjects([link, obj.parent], True, 1) if obj.parent.phobostype == 'link': bpy.ops.object.parent_set(type='BONE_RELATIVE') else: bpy.ops.object.parent_set(type='OBJECT') children = selectionUtils.getImmediateChildren(obj) if parentobjects: children.append(obj) for child in children: selectionUtils.selectObjects([child], True, 0) bpy.ops.object.parent_clear(type='CLEAR_KEEP_TRANSFORM') selectionUtils.selectObjects([child, link], True, 1) bpy.ops.object.parent_set(type='BONE_RELATIVE')
def initObjectProperties(obj, phobostype=None, ignoretypes=(), includeannotations=True, ignorename=False): """Initializes phobos dictionary of *obj*, including information stored in custom properties. Args: obj(bpy_types.Object): object to derive initial properties from. phobostype(str, optional): limit parsing of data fields to this phobostype ignoretypes(list, optional): list of properties ignored while initializing the objects properties. ignorename(bool, optional): whether or not to add the object's name Returns: dict """ # allow duplicated names differentiated by types props = {} if ignorename else { 'name': nUtils.getObjectName(obj, phobostype) } # if no phobostype is defined, everything is parsed if not phobostype: for key, value in obj.items(): props[key] = value # search for type-specific properties if phobostype is defined else: for key, value in obj.items(): # transform Blender id_arrays into lists if hasattr(value, 'to_list'): value = list(value) if key.startswith(phobostype + '/'): if key.count('/') == 1: props[key.replace(phobostype + '/', '')] = value elif key.count('/') == 2: category, specifier = key.split('/')[1:] if '$' + category not in props: props['$' + category] = {} props['$' + category][specifier] = value # ignore two-level specifiers if phobostype is not present elif key.count('/') == 1: category, specifier = key.split('/') if category not in ignoretypes: if '$' + category not in props: props['$' + category] = {} props['$' + category][specifier] = value if includeannotations: annotationobjs = sUtils.getImmediateChildren(obj, ('annotation', ), selected_only=True) for obj in annotationobjs: props.update( initObjectProperties(obj, phobostype, ignoretypes, includeannotations, ignorename=True)) return props
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 getInertiaRelevantObjects(link, selected_only=False): """Returns a list of visual and collision objects of a link. If name-pairs of visual and collision objects are detected, the one with the latest change-date is used. If this is not clear, visual objects are omitted in favor of collision objects. If the selected_only parameter is used, only the selected objects are considered. Args: link(bpy_types.Object): The link you want to gather the inertia relevant objects for. selected_only(bool, optional): return only relevant objects which are selected (Default value = False) Returns: list """ objdict = { obj.name: obj for obj in sUtils.getImmediateChildren(link, ['visual', 'collision'], selected_only) } basenames = set() inertiaobjects = [] for objname in objdict.keys(): if 'mass' in objdict[objname]: if not objname.startswith('visual_') and not objname.startswith( 'collision_'): inertiaobjects.append(objdict[objname]) else: basename = objname.replace(objdict[objname].phobostype + '_', '') if basename not in basenames: basenames.add(basename) collision = 'collision_'+basename if 'collision_'+basename in objdict.keys()\ and 'mass' in objdict['collision_'+basename] else None visual = 'visual_'+basename if 'visual_'+basename in objdict.keys()\ and 'mass' in objdict['visual_'+basename] else None if visual and collision: try: tv = gUtils.datetimeFromIso( objdict[visual]['masschanged']) tc = gUtils.datetimeFromIso( objdict[collision]['masschanged']) # if collision information is older than visual information if tc < tv: inertiaobjects.append(objdict[visual]) else: inertiaobjects.append(objdict[collision]) # if masschanged not present in both except KeyError: inertiaobjects.append(objdict[collision]) else: inertiaobjects.append(objdict[collision] if collision else objdict[visual]) return inertiaobjects
def deriveLinkfromObject( obj, scale=0.2, parenting=True, parentobjects=False, namepartindices=[], separator="_", prefix="link" ): """Derives a link from an object that defines a joint through its position, orientation and parent-child relationships. :param obj: The object you want to derive your link from. :type obj: bpy_types.Object :param scale: The scale you want to apply to the link. :type scale: float :param parenting: Whether you want to automate the parenting of the new link or not. :type parenting: bool. :param parentobjects: Whether you want to parent all the objects to the new link or not. :type parentobjects: bool. :param namepartindices: Parts of the objects name you want to reuse in the links name. :type namepartindices: list with two elements. :param separator: The separator you want to use to separate the links name with. Its '_' per default :type separator: str :param prefix: The prefix you want to use for the new links name. Its 'link' per default. :type prefix: str """ print("Deriving link from", namingUtils.getObjectName(obj)) nameparts = namingUtils.getObjectName(obj).split("_") rotation = obj.matrix_world.to_euler() if "invertAxis" in obj and obj["invertAxis"] == 1: rotation.x += math.pi if rotation.x < 0 else -math.pi tmpname = namingUtils.getObjectName(obj) if namepartindices: try: tmpname = separator.join([nameparts[p] for p in namepartindices]) except IndexError: print("Wrong name segment indices given for obj", namingUtils.getObjectName(obj)) if prefix != "": tmpname = prefix + separator + tmpname if tmpname == namingUtils.getObjectName(obj): obj.name += "*" link = createLink(scale, obj.matrix_world.to_translation(), obj.matrix_world.to_euler(), tmpname) if parenting: if obj.parent: selectionUtils.selectObjects([link, obj.parent], True, 1) if obj.parent.phobostype == "link": bpy.ops.object.parent_set(type="BONE_RELATIVE") else: bpy.ops.object.parent_set(type="OBJECT") children = selectionUtils.getImmediateChildren(obj) if parentobjects: children.append(obj) for child in children: selectionUtils.selectObjects([child], True, 0) bpy.ops.object.parent_clear(type="CLEAR_KEEP_TRANSFORM") selectionUtils.selectObjects([child, link], True, 1) bpy.ops.object.parent_set(type="BONE_RELATIVE")
def createInertials(link, empty=False, preserve_children=False): # create inertial representations for visual and collision objects in link viscols = getInertiaRelevantObjects(link) # clean existing data if not preserve_children: oldinertials = selectionUtils.getImmediateChildren(link, ['inertial']) else: try: oldinertials = [bpy.data.objects['inertial_'+link.name]] except KeyError: oldinertials = None if oldinertials: selectionUtils.selectObjects(oldinertials, clear=True, active=0) bpy.ops.object.delete() if not preserve_children: for obj in viscols: if not empty: mass = obj['mass'] if 'mass' in obj else None geometry = robotdictionary.deriveGeometry(obj) if mass is not None: inert = calculateInertia(mass, geometry) if inert is not None: inertial = createInertial(obj) inertial['mass'] = mass inertial['inertia'] = inert else: createInertial(obj) # compose inertial object for link if not empty: mass, com, inert = fuseInertiaData(selectionUtils.getImmediateChildren(link, ['inertial'])) if mass and com and inert: inertial = createInertial(link) com_translate = mathutils.Matrix.Translation(com) inertial.matrix_local = com_translate bpy.ops.transform.translate(value=(0, 0, 0)) # FIXME: this is a trick to force Blender to apply matrix_local inertial['inertial/mass'] = mass inertial['inertial/inertia'] = inertiaMatrixToList(inert) else: createInertial(link)
def addPCCombinations(parent): """Function to add parent/child link combinations for all parents an children that are not already set via collision bitmask. :param parent: This is the parent object. :type parent: dict. """ children = selectionUtils.getImmediateChildren(parent, 'link') if len(children) > 0: for child in children: #output.append(xmlline(2, 'disable_collisions', ('link1', 'link2'), (mother.name, child.name))) if ((parent, child) not in collisionExclusives) or ((child, parent) not in collisionExclusives): collisionExclusives.append((parent.name, child.name)) addPCCombinations(child)
def getInertiaRelevantObjects(link): """Returns a list of visual and collision objects of a link. If name-pairs of visual and collision objects are detected, the one with the latest change-date is used. If this is not clear, visual objects are omitted in favor of collision objects. :param link: The link you want to gather the inertia relevant objects for. :type link: bpy_types.Object :return: list """ objdict = { obj.name: obj for obj in selectionUtils.getImmediateChildren(link, ['visual', 'collision']) } basenames = set() inertiaobjects = [] for objname in objdict.keys(): if 'mass' in objdict[objname]: if not objname.startswith('visual_') and not objname.startswith( 'collision_'): inertiaobjects.append(objdict[objname]) else: basename = objname.replace(objdict[objname].phobostype + '_', '') if not basename in basenames: basenames.add(basename) collision = 'collision_'+basename if 'collision_'+basename in objdict.keys()\ and 'mass' in objdict['collision_'+basename] else None visual = 'visual_'+basename if 'visual_'+basename in objdict.keys()\ and 'mass' in objdict['visual_'+basename] else None if visual and collision: try: tv = generalUtils.datetimeFromIso( objdict[visual]['masschanged']) tc = generalUtils.datetimeFromIso( objdict[collision]['masschanged']) if tc < tv: # if collision information is older than visual information inertiaobjects.append(objdict[visual]) else: inertiaobjects.append(objdict[collision]) except KeyError: # if masschanged not present in both inertiaobjects.append(objdict[collision]) else: inertiaobjects.append(objdict[collision] if collision else objdict[visual]) return inertiaobjects
def calculateMassOfLink(link): """Calculates the masses of visual and collision objects found in a link, compares it to mass in link inertial object if present and returns the max of both, warning if they are not equal. :param link: The link you want to calculate the visuals and collision objects mass of. :type link: dict :return: double """ objects = getInertiaRelevantObjects(link, ['visual', 'collision']) inertials = sUtils.getImmediateChildren(link, ['inertial']) objectsmass = sUtils.calculateSum(objects, 'mass') if len(inertials) == 1: inertialmass = inertials[0]['mass'] if 'mass' in inertials[0] else 0 if objectsmass != inertialmass: log("Warning: Masses are inconsistent, sync masses of link!", "WARNING") return max(objectsmass, inertialmass)
def calculateMassOfLink(link): """Calculates the masses of visual and collision objects found in a link, compares it to mass in link inertial object if present and returns the max of both, warning if they are not equal. :param link: The link you want to calculate the visuals and collision objects mass of. :type link: dict. :return: double. """ objects = getInertiaRelevantObjects(link, ['visual', 'collision']) inertials = selectionUtils.getImmediateChildren(link, ['inertial']) objectsmass = selectionUtils.calculateSum(objects, 'mass') if len(inertials) == 1: inertialmass = inertials[0]['mass'] if 'mass' in inertials[0] else 0 if objectsmass != inertialmass: log("Warning: Masses are inconsistent, sync masses of link!", "WARNING") return max(objectsmass, inertialmass)
def getInertiaRelevantObjects(link): """Returns a list of visual and collision objects of a link. If name-pairs of visual and collision objects are detected, the one with the latest change-date is used. If this is not clear, visual objects are omitted in favor of collision objects. :param link: The link you want to gather the inertia relevant objects for. :type link: Blender object. :return: list. """ objdict = {obj.name: obj for obj in selectionUtils.getImmediateChildren(link, ['visual', 'collision'])} basenames = set() inertiaobjects = [] for objname in objdict.keys(): if 'mass' in objdict[objname]: if not objname.startswith('visual_') and not objname.startswith('collision_'): inertiaobjects.append(objdict[objname]) else: basename = objname.replace(objdict[objname].phobostype + '_', '') if not basename in basenames: basenames.add(basename) collision = 'collision_'+basename if 'collision_'+basename in objdict.keys()\ and 'mass' in objdict['collision_'+basename] else None visual = 'visual_'+basename if 'visual_'+basename in objdict.keys()\ and 'mass' in objdict['visual_'+basename] else None if visual and collision: try: tv = generalUtils.datetimeFromIso(objdict[visual]['masschanged']) tc = generalUtils.datetimeFromIso(objdict[collision]['masschanged']) if tc < tv: # if collision information is older than visual information inertiaobjects.append(objdict[visual]) else: inertiaobjects.append(objdict[collision]) except KeyError: # if masschanged not present in both inertiaobjects.append(objdict[collision]) else: inertiaobjects.append(objdict[collision] if collision else objdict[visual]) return inertiaobjects
def buildRobotDictionary(): """Builds a python dictionary representation of a Blender robot model for export and inspection. """ objectlist = bpy.context.selected_objects #notifications, faulty_objects = robotupdate.updateModel(bpy.context.selected_objects) #print(notifications) robot = { 'links': {}, 'joints': {}, 'sensors': {}, 'motors': {}, 'controllers': {}, 'materials': {}, 'groups': {}, 'chains': {}, 'lights': {} } #save timestamped version of model robot["date"] = datetime.now().strftime("%Y%m%d_%H:%M") root = selectionUtils.getRoot(bpy.context.selected_objects[0]) if root.phobostype != 'link': raise Exception("Found no 'link' object as root of the robot model.") else: if 'modelname' in root: robot['modelname'] = root["modelname"] else: robot['modelname'] = 'unnamed_robot' # digest all the links to derive link and joint information print('\nParsing links, joints and motors...') for obj in bpy.context.selected_objects: if obj.phobostype == 'link': link, joint, motor = deriveKinematics(obj) robot['links'][namingUtils.getObjectName( obj, phobostype="link" )] = link # it's important that this is really the object's name if joint: # joint can be None if link is a root robot['joints'][joint['name']] = joint if motor: robot['motors'][joint['name']] = motor obj.select = False # add inertial information to link print('\n\nParsing inertials...') for l in robot['links']: #link = bpy.data.objects[l] NEW NAMING! link = selectionUtils.getObjectByName( l)[0] if selectionUtils.getObjectByName( l) is not None else "ERROR!" inertials = selectionUtils.getImmediateChildren(link, ['inertial']) if len(inertials) == 1: props, parent = deriveDictEntry(inertials[0]) if not ( props is None or parent is None ): # this may be the case if there is inertia information missing robot['links'][namingUtils.getObjectName( parent)]['inertial'] = props inertials[0].select = False elif len(inertials) > 1: for i in inertials: if i.name == 'inertial_' + l: props, parent = deriveDictEntry(i) robot['links'][namingUtils.getObjectName( parent, phobostype="link")]['inertial'] = props # FIXME: this has to be re-implemented #if linkinertial == None: # mass, com, inertia = inertia.fuseInertiaData(inertials) # parent = inertials[0].parent # matrix_local = mathutils.Matrix.Translation(mathutils.Vector(com)) # pose = {} # pose['matrix'] = [list(vector) for vector in list(matrix_local)] # pose['translation'] = list(matrix_local.to_translation()) # pose['rotation_euler'] = list(matrix_local.to_euler()) # pose['rotation_quaternion'] = list(matrix_local.to_quaternion()) # props = {'mass': mass, 'pose': pose, 'inertia': inertia} # robot['links'][parent.name]['inertial'] = props for i in inertials: i.select = False # complete link information by parsing visuals and collision objects print('\n\nParsing visual and collision (approximation) objects...') capsules_list = [] for obj in bpy.context.selected_objects: print("Parsing object " + namingUtils.getObjectName(obj)) if obj.phobostype in ['visual', 'collision']: props, parent = deriveDictEntry(obj) if all([ key in props for key in ['cylinder', 'sphere1', 'sphere2'] ]): # this is the case with simulated capsules capsules_list.append({ 'link': parent.name, 'name': props['cylinder']['name'][:-len('_cylinder')], 'radius': props['cylinder']['geometry']['radius'], 'length': props['cylinder']['geometry']['length'] + 2 * props['cylinder']['geometry']['radius'], #'bitmask': props['cylinder']['bitmask'] }) for key in props: robot['links'][namingUtils.getObjectName( parent, phobostype="link")][obj.phobostype][key] = props[key] else: robot['links'][namingUtils.getObjectName( parent, phobostype="link")][obj.phobostype][ namingUtils.getObjectName( obj, phobostype=obj.phobostype)] = props obj.select = False elif obj.phobostype == 'approxsphere': props, parent = deriveDictEntry(obj) robot['links'][namingUtils.getObjectName( parent)]['approxcollision'].append(props) obj.select = False robot['capsules'] = capsules_list # 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 print('\n\nParsing sensors and controllers...') for obj in bpy.context.selected_objects: if obj.phobostype in ['sensor', 'controller']: robot[obj.phobostype + 's'][namingUtils.getObjectName(obj)] = deriveDictEntry(obj) obj.select = False # parse materials print('\n\nParsing materials...') robot['materials'] = collectMaterials(objectlist) for obj in objectlist: if obj.phobostype == 'visual' and len(obj.data.materials) > 0: mat = obj.data.materials[0] if not namingUtils.getObjectName(mat) in robot['materials']: robot['materials'][namingUtils.getObjectName( mat)] = deriveMaterial( mat) #this should actually never happen robot['links'][namingUtils.getObjectName( obj.parent)]['visual'][namingUtils.getObjectName( obj, phobostype="visual" )]['material'] = namingUtils.getObjectName(mat) # gather information on groups of objects print('\n\nParsing groups...') for group in bpy.data.groups: # TODO: get rid of the "data" part if len(group.objects) > 0 and namingUtils.getObjectName( group) != "RigidBodyWorld": robot['groups'][namingUtils.getObjectName( group)] = deriveGroupEntry(group) # gather information on chains of objects print('\n\nParsing chains...') chains = [] for obj in bpy.data.objects: if obj.phobostype == 'link' and 'endChain' in obj: chains.extend(deriveChainEntry(obj)) for chain in chains: robot['chains'][chain['name']] = chain # gather information on global lights print('\n\nParsing lights...') for obj in bpy.context.selected_objects: if obj.phobostype == 'light': robot['lights'][namingUtils.getObjectName(obj)] = deriveLight(obj) robot['poses'] = deriveStoredPoses() #shorten numbers in dictionary to n decimalPlaces and return it print('\n\nRounding numbers...') epsilon = 10**(-bpy.data.worlds[0].decimalPlaces ) # TODO: implement this separately return generalUtils.epsilonToZero(robot, epsilon, bpy.data.worlds[0].decimalPlaces)
def buildRobotDictionary(): """Builds a python dictionary representation of a Blender robot model for export and inspection.""" objectlist = bpy.context.selected_objects #notifications, faulty_objects = robotupdate.updateModel(bpy.context.selected_objects) #print(notifications) robot = {'links': {}, 'joints': {}, 'sensors': {}, 'motors': {}, 'controllers': {}, 'materials': {}, 'groups': {}, 'chains': {}, 'lights': {} } #save timestamped version of model robot["date"] = datetime.now().strftime("%Y%m%d_%H:%M") root = selectionUtils.getRoot(bpy.context.selected_objects[0]) if root.phobostype != 'link': raise Exception("Found no 'link' object as root of the robot model.") else: if 'modelname' in root: robot['modelname'] = root["modelname"] else: robot['modelname'] = 'unnamed_robot' # digest all the links to derive link and joint information print('\nParsing links, joints and motors...') for obj in bpy.context.selected_objects: if obj.phobostype == 'link': link, joint, motor = deriveKinematics(obj) robot['links'][namingUtils.getObjectName(obj, phobostype="link")] = link # it's important that this is really the object's name if joint: # joint can be None if link is a root robot['joints'][joint['name']] = joint if motor: robot['motors'][joint['name']] = motor obj.select = False # add inertial information to link print('\n\nParsing inertials...') for l in robot['links']: #link = bpy.data.objects[l] NEW NAMING! link = selectionUtils.getObjectByName(l)[0] if selectionUtils.getObjectByName(l) is not None else "ERROR!" inertials = selectionUtils.getImmediateChildren(link, ['inertial']) if len(inertials) == 1: props, parent = deriveDictEntry(inertials[0]) if not (props is None or parent is None): # this may be the case if there is inertia information missing robot['links'][namingUtils.getObjectName(parent)]['inertial'] = props inertials[0].select = False elif len(inertials) > 1: for i in inertials: if namingUtils.getObjectName(i, phobostype="inertial") == 'inertial_' + l: props, parent = deriveDictEntry(i) robot['links'][namingUtils.getObjectName(parent, phobostype="link")]['inertial'] = props # FIXME: this has to be re-implemented #if linkinertial == None: # mass, com, inertia = inertia.fuseInertiaData(inertials) # parent = inertials[0].parent # matrix_local = mathutils.Matrix.Translation(mathutils.Vector(com)) # pose = {} # pose['matrix'] = [list(vector) for vector in list(matrix_local)] # pose['translation'] = list(matrix_local.to_translation()) # pose['rotation_euler'] = list(matrix_local.to_euler()) # pose['rotation_quaternion'] = list(matrix_local.to_quaternion()) # props = {'mass': mass, 'pose': pose, 'inertia': inertia} # robot['links'][parent.name]['inertial'] = props for i in inertials: i.select = False # complete link information by parsing visuals and collision objects print('\n\nParsing visual and collision (approximation) objects...') for obj in bpy.context.selected_objects: print("Parsing object " + namingUtils.getObjectName(obj)) if obj.phobostype in ['visual', 'collision']: props, parent = deriveDictEntry(obj) robot['links'][namingUtils.getObjectName(parent, phobostype="link")][obj.phobostype][namingUtils.getObjectName(obj, phobostype=obj.phobostype)] = props obj.select = False elif obj.phobostype == 'approxsphere': props, parent = deriveDictEntry(obj) robot['links'][namingUtils.getObjectName(parent)]['approxcollision'].append(props) obj.select = False # 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 print('\n\nParsing sensors and controllers...') for obj in bpy.context.selected_objects: if obj.phobostype in ['sensor', 'controller']: robot[obj.phobostype+'s'][namingUtils.getObjectName(obj)] = deriveDictEntry(obj) obj.select = False # parse materials print('\n\nParsing materials...') robot['materials'] = collectMaterials(objectlist) for obj in objectlist: if obj.phobostype == 'visual' and len(obj.data.materials) > 0: mat = obj.data.materials[0] if not namingUtils.getObjectName(mat) in robot['materials']: robot['materials'][namingUtils.getObjectName(mat)] = deriveMaterial(mat) #this should actually never happen robot['links'][namingUtils.getObjectName(obj.parent)]['visual'][namingUtils.getObjectName(obj, phobostype="visual")]['material'] = namingUtils.getObjectName(mat) # gather information on groups of objects print('\n\nParsing groups...') for group in bpy.data.groups: # TODO: get rid of the "data" part if len(group.objects) > 0 and namingUtils.getObjectName(group) != "RigidBodyWorld": robot['groups'][namingUtils.getObjectName(group)] = deriveGroupEntry(group) # gather information on chains of objects print('\n\nParsing chains...') chains = [] for obj in bpy.data.objects: if obj.phobostype == 'link' and 'endChain' in obj: chains.extend(deriveChainEntry(obj)) for chain in chains: robot['chains'][chain['name']] = chain # gather information on global lights print('\n\nParsing lights...') for obj in bpy.context.selected_objects: if obj.phobostype == 'light': robot['lights'][namingUtils.getObjectName(obj)] = deriveLight(obj) robot['poses'] = deriveStoredPoses() #shorten numbers in dictionary to n decimalPlaces and return it print('\n\nRounding numbers...') epsilon = 10**(-bpy.data.worlds[0].decimalPlaces) # TODO: implement this separately return generalUtils.epsilonToZero(robot, epsilon, bpy.data.worlds[0].decimalPlaces)
def deriveEntity(entity, outpath): """This function handles a heightmap entity in a scene to export it Args: smurf(bpy.types.Object): The heightmap root object. outpath(str): The path to export to. savetosubfolder(bool): If True data will be exported into subfolders. entity: Returns: : dict - An entry for the scenes entitiesList """ heightmap = entity # determine outpath for the heightmap export heightmap_outpath = securepath(os.path.join(outpath, structure_subfolder)) log("Exporting " + heightmap["entity/name"] + " as a heightmap entity", "INFO") entitypose = models.deriveObjectPose(heightmap) heightmapMesh = sUtils.getImmediateChildren(heightmap)[0] if bpy.data.window_managers[0].heightmapMesh: exMesh = heightmapMesh.to_mesh(bpy.context.scene, True, "PREVIEW") exMesh.name = "hm_" + heightmap["entity/name"] oldMesh = heightmapMesh.data heightmapMesh.data = exMesh heightmapMesh.modifiers["displace_heightmap"].show_render = False heightmapMesh.modifiers["displace_heightmap"].show_viewport = False # CHECK are the heightmaps exported to the right directory? if bpy.data.window_managers[0].useObj: ioUtils.exportObj(heightmap_outpath, heightmapMesh) filename = os.path.join("heightmaps", exMesh.name + ".obj") elif bpy.data.window_managers[0].useStl: ioUtils.exportStl(heightmap_outpath, heightmapMesh) filename = os.path.join("heightmaps", exMesh.name + ".stl") elif bpy.data.window_managers[0].useDae: ioUtils.exportDae(heightmap_outpath, heightmapMesh) filename = os.path.join("heightmaps", exMesh.name + ".dae") else: log("No mesh export type checked! Aborting heightmap export.", "ERROR") return {} heightmapMesh.modifiers["displace_heightmap"].show_render = True heightmapMesh.modifiers["displace_heightmap"].show_viewport = True heightmapMesh.data = oldMesh bpy.data.meshes.remove(exMesh) entry = { "name": heightmap["entity/name"], "type": "mesh", "file": filename, "anchor": heightmap["anchor"] if "anchor" in heightmap else "none", "position": entitypose["translation"], "rotation": entitypose["rotation_quaternion"], } else: imagepath = os.path.abspath( os.path.join(os.path.split(bpy.data.filepath)[0], heightmap["image"]) ) shutil.copy2(imagepath, heightmap_outpath) entry = { "name": heightmap["entity/name"], "type": "heightmap", "file": os.path.join("heightmaps", os.path.basename(imagepath)), "anchor": heightmap["anchor"] if "anchor" in heightmap else "none", "width": heightmapMesh.dimensions[1], "length": heightmapMesh.dimensions[0], "height": heightmapMesh.modifiers["displace_heightmap"].strength, "position": entitypose["translation"], "rotation": entitypose["rotation_quaternion"], } return entry
def deriveMotor(obj, jointdict=None): """Derives motor information from an object. Args: obj(bpy_types.Object): Blender object to derive the motor from jointdict(dict, optional): phobos representation of the respective joint (Default value = None) Returns: : dict -- phobos representation of a motor """ import phobos.model.models as models import phobos.model.controllers as controllermodel props = models.initObjectProperties(obj, phobostype='motor') # return None if no motor is attached (there will always be at least a name in the props) if len(props) < 2: return None # make sure the parent is a joint if not obj.parent or obj.parent.phobostype != 'link' or 'joint/type' not in obj.parent: log( "Can not derive motor from {}. Insufficient requirements from parent object!" .format(obj.name), 'ERROR', ) return None props['joint'] = nUtils.getObjectName(obj.parent, phobostype='joint') # todo: transfer joint limits to motor properties # check for a mimic motor for k in (obj.parent).keys(): # Check for mimic motor if "mimic" in k: # Find the name mimic_driver = sUtils.getObjectByName( (obj.parent)['joint/mimic_joint'], phobostypes=['link']) c_motor = sUtils.getImmediateChildren(mimic_driver, phobostypes=['motor']) props['mimic_motor'] = nUtils.getObjectName(c_motor[0], phobostype='motor') props['mimic_multiplier'] = (obj.parent)['joint/mimic_multiplier'] props['mimic_offset'] = (obj.parent)['joint/mimic_offset'] break # try to derive the motor controller controllerobjs = [ control for control in obj.children if control.phobostype == 'controller' ] if controllerobjs: controller = controllermodel.deriveController(controllerobjs[0]) else: controller = None # assign the derived controller if controller: props['controller'] = controller['name'] else: del props['controller'] return props
def deriveEntity(entity, outpath, savetosubfolder): """This function handles a heightmap entity in a scene to export it :param smurf: The heightmap root object. :type smurf: bpy.types.Object :param outpath: The path to export to. :type outpath: str :param savetosubfolder: If True data will be exported into subfolders. :type savetosubfolder: bool :return: dict - An entry for the scenes entitiesList """ heightmap = entity # determine outpath for the heightmap export heightmap_outpath = securepath( os.path.join(outpath, structure_subfolder ) if savetosubfolder else outpath) log("Exporting " + heightmap["entity/name"] + " as a heightmap entity", "INFO") entitypose = models.deriveObjectPose(heightmap) heightmapMesh = sUtils.getImmediateChildren(heightmap)[0] if bpy.data.worlds[0].heightmapMesh: exMesh = heightmapMesh.to_mesh(bpy.context.scene, True, "PREVIEW") exMesh.name = "hm_" + heightmap["entity/name"] oldMesh = heightmapMesh.data heightmapMesh.data = exMesh heightmapMesh.modifiers["displace_heightmap"].show_render = False heightmapMesh.modifiers["displace_heightmap"].show_viewport = False if bpy.data.worlds[0].useObj: iUtils.exportObj(heightmap_outpath, heightmapMesh) filename = os.path.join("heightmaps", exMesh.name + ".obj") elif bpy.data.worlds[0].useStl: iUtils.exportStl(heightmap_outpath, heightmapMesh) filename = os.path.join("heightmaps", exMesh.name + ".stl") elif bpy.data.worlds[0].useDae: iUtils.exportDae(heightmap_outpath, heightmapMesh) filename = os.path.join("heightmaps", exMesh.name + ".dae") else: log("No mesh export type checked! Aborting heightmap export.", "ERROR", __name__ + ".handleScene_heightmap") return {} heightmapMesh.modifiers["displace_heightmap"].show_render = True heightmapMesh.modifiers["displace_heightmap"].show_viewport = True heightmapMesh.data = oldMesh bpy.data.meshes.remove(exMesh) entry = { "name": heightmap["entity/name"], "type": "mesh", "file": filename, "anchor": heightmap["anchor"] if "anchor" in heightmap else "none", "position": entitypose["translation"], "rotation": entitypose["rotation_quaternion"] } else: imagepath = os.path.abspath( os.path.join( os.path.split(bpy.data.filepath)[0], heightmap["image"])) shutil.copy2(imagepath, heightmap_outpath) entry = { "name": heightmap["entity/name"], "type": "heightmap", "file": os.path.join("heightmaps", os.path.basename(imagepath)), "anchor": heightmap["anchor"] if "anchor" in heightmap else "none", "width": heightmapMesh.dimensions[1], "length": heightmapMesh.dimensions[0], "height": heightmapMesh.modifiers["displace_heightmap"].strength, "position": entitypose["translation"], "rotation": entitypose["rotation_quaternion"] } return entry
def initObjectProperties( obj, phobostype=None, ignoretypes=(), includeannotations=True, ignorename=False ): """Initializes phobos dictionary of *obj*, including information stored in custom properties. Args: obj(bpy_types.Object): object to derive initial properties from. phobostype(str, optional): limit parsing of data fields to this phobostype (Default value = None) ignoretypes(list, optional): list of properties ignored while initializing the objects properties. (Default value = ()) ignorename(bool, optional): whether or not to add the object's name (Default value = False) includeannotations: (Default value = True) Returns: : dict -- phobos properties of the object """ # allow duplicated names differentiated by types props = {} if ignorename else {'name': nUtils.getObjectName(obj, phobostype)} # if no phobostype is defined, everything is parsed if not phobostype: for key, value in obj.items(): # transform Blender id_arrays into lists if hasattr(value, 'to_list'): value = list(value) elif hasattr(value, 'to_dict'): value = value.to_dict() props[key] = value # search for type-specific properties if phobostype is defined else: for key, value in obj.items(): # transform Blender id_arrays into lists if hasattr(value, 'to_list'): value = list(value) elif hasattr(value, 'to_dict'): value = value.to_dict() # remove phobostype namespaces for the object if key.startswith(phobostype + '/'): if key.count('/') == 1: props[key.replace(phobostype + '/', '')] = value # TODO make this work for all levels of hierarchy elif key.count('/') == 2: category, specifier = key.split('/')[1:] if '$' + category not in props: props['$' + category] = {} props['$' + category][specifier] = value # ignore two-level specifiers if phobostype is not present elif key.count('/') == 1: category, specifier = key.split('/') if category not in ignoretypes: if '$' + category not in props: props['$' + category] = {} props['$' + category][specifier] = value # collect phobostype specific annotations from child objects if includeannotations: annotationobjs = sUtils.getImmediateChildren(obj, ('annotation',), selected_only=True) for annot in annotationobjs: log( " Adding annotations from {}.".format( nUtils.getObjectName(annot, phobostype='annotation') ), 'DEBUG', ) props.update( initObjectProperties( annot, phobostype, ignoretypes, includeannotations, ignorename=True ) ) # recursively enrich the property dictionary props = recursive_dictionary_cleanup(props) return props