def getExportModels(): """Returns a list of objects representing a model (root) in the Blender scene""" if getExpSettings().selectedOnly: roots = [root for root in sUtils.getRoots() if root.select] else: roots = sUtils.getRoots() return list(roots)
def getExportModels(): # DOCU add some docstring if getExpSettings().selectedOnly: roots = {root for root in sUtils.getRoots() if root.select} else: roots = sUtils.getRoots() return list(roots)
def storePose(modelname, posename): """ Stores the current pose of all of a robot's selected joints. Existing poses of the same name will be overwritten. :param modelname: The robot the pose belongs to. :type modelname: str. :param posename: The name the pose will be stored under. :type posename: str. :return: Nothing. """ rootlink = None for root in sUtils.getRoots(): if root['modelname'] == modelname: rootlink = root if rootlink: filename = modelname + '::poses' posedict = yaml.load(bUtils.readTextFile(filename)) if not posedict: posedict = {posename: {'name': posename, 'joints': {}}} else: posedict[posename] = {'name': posename, 'joints': {}} bpy.ops.object.mode_set(mode='POSE') links = sUtils.getChildren(rootlink, ('link', ), True, False) for link in (link for link in links if 'joint/type' in link and link['joint/type'] not in ['fixed', 'floating']): link.pose.bones['Bone'].rotation_mode = 'XYZ' posedict[posename]['joints'][nUtils.getObjectName( link, 'joint')] = link.pose.bones['Bone'].rotation_euler.y bUtils.updateTextFile(filename, yaml.dump(posedict, default_flow_style=False)) else: log("No model root could be found to store the pose for", "ERROR")
def storePose(modelname, posename): """ Stores the current pose of all of a robot's selected joints. Existing poses of the same name will be overwritten. :param modelname: The robot the pose belongs to. :type modelname: str. :param posename: The name the pose will be stored under. :type posename: str. :return: Nothing. """ rootlink = None for root in sUtils.getRoots(): if root['modelname'] == modelname: rootlink = root if rootlink: filename = modelname + '::poses' posedict = yaml.load(bUtils.readTextFile(filename)) if not posedict: posedict = {posename: {'name': posename, 'joints': {}}} else: posedict[posename] = {'name': posename, 'joints': {}} bpy.ops.object.mode_set(mode='POSE') links = sUtils.getChildren(rootlink, ('link',), True, False) for link in (link for link in links if 'joint/type' in link and link['joint/type'] not in ['fixed', 'floating']): link.pose.bones['Bone'].rotation_mode = 'XYZ' posedict[posename]['joints'][nUtils.getObjectName(link, 'joint')] = link.pose.bones['Bone'].rotation_euler.y bUtils.updateTextFile(filename, yaml.dump(posedict, default_flow_style=False)) else: log("No model root could be found to store the pose for", "ERROR", "storePose")
def execute(self, context): """ Args: context: Returns: """ selection = [] if self.modelname: log("phobos: Selecting model" + self.modelname, "INFO") roots = sUtils.getRoots() for root in roots: if nUtils.getModelName(root) == self.modelname: selection = sUtils.getChildren(root) else: log("No model name provided, deriving from selection...", "INFO") roots = set() for obj in bpy.context.selected_objects: roots.add(sUtils.getRoot(obj)) for root in list(roots): selection.extend(sUtils.getChildren(root)) sUtils.selectObjects(list(selection), True) return {'FINISHED'}
def storePose(robot_name, pose_name): """ Store the current pose of all of a robot's selected links. Existing poses of the same name will be overwritten. :param robot_name: The robot the pose belongs to. :type robot_name: str. :param pose_name: The name the pose will be stored under. :type pose_name: str. :return: Nothing. """ file_name = 'robot_poses_' + robot_name load_file = blenderUtils.readTextFile(file_name) if load_file == '': poses = {} else: poses = yaml.load(load_file) new_pose = {} prev_mode = bpy.context.mode bpy.ops.object.mode_set(mode='POSE') for root in selectionUtils.getRoots(): if root['modelname'] == robot_name: links = selectionUtils.getChildren(root) for link in links: if link.select and link.phobostype == 'link': link.pose.bones['Bone'].rotation_mode = 'XYZ' new_pose[namingUtils.getObjectName(link, 'joint')] = link.pose.bones['Bone'].rotation_euler.y bpy.ops.object.mode_set(mode=prev_mode) poses[pose_name] = new_pose blenderUtils.updateTextFile(file_name, yaml.dump(poses))
def storePose(robot_name, pose_name): """ Store the current pose of all of a robot's selected links. Existing poses of the same name will be overwritten. :param robot_name: The robot the pose belongs to. :type robot_name: str. :param pose_name: The name the pose will be stored under. :type pose_name: str. :return: Nothing. """ file_name = 'robot_poses_' + robot_name load_file = blenderUtils.readTextFile(file_name) if load_file == '': poses = {} else: poses = yaml.load(load_file) new_pose = {} prev_mode = bpy.context.mode bpy.ops.object.mode_set(mode='POSE') for root in selectionUtils.getRoots(): if root['modelname'] == robot_name: links = selectionUtils.getChildren(root) for link in links: if link.select and link.phobostype == 'link': link.pose.bones['Bone'].rotation_mode = 'XYZ' new_pose[namingUtils.getObjectName( link, 'joint')] = link.pose.bones['Bone'].rotation_euler.y bpy.ops.object.mode_set(mode=prev_mode) poses[pose_name] = new_pose blenderUtils.updateTextFile(file_name, yaml.dump(poses))
def execute(self, context): selection = [] if self.modelname: print("phobos: Selecting model", self.modelname) roots = selectionUtils.getRoots() for root in roots: if root["modelname"] == self.modelname: selection = selectionUtils.getChildren(root) else: print("phobos: No model name provided, deriving from selection...") roots = set() for obj in bpy.context.selected_objects: print("Selecting", selectionUtils.getRoot(obj).name) roots.add(selectionUtils.getRoot(obj)) for root in list(roots): selection.extend(selectionUtils.getChildren(root)) selectionUtils.selectObjects(list(selection), True) return {'FINISHED'}
def execute(self, context): selection = [] if self.modelname: print("phobos: Selecting model", self.modelname) roots = sUtils.getRoots() for root in roots: if root["modelname"] == self.modelname: selection = sUtils.getChildren(root) else: print("phobos: No model name provided, deriving from selection...") roots = set() for obj in bpy.context.selected_objects: print("Selecting", sUtils.getRoot(obj).name) roots.add(sUtils.getRoot(obj)) for root in list(roots): selection.extend(sUtils.getChildren(root)) sUtils.selectObjects(list(selection), True) return {'FINISHED'}
def get_robot_names(scene, context): robot_names = [(root['modelname'], ) * 3 for root in selectionUtils.getRoots()] return robot_names
def get_robot_names(scene, context): robot_names = [(root['modelname'],)*3 for root in selectionUtils.getRoots()] return robot_names
def execute(self, context): materials.createPhobosMaterials() # TODO: this should move to initialization robotupdate.updateModels(selectionUtils.getRoots(), self.property_fix) return {'FINISHED'}
def exportSMURFsScene(selected_only=True, subfolders=True): #TODO: Refactoring needed!!! """Exports all robots in a scene in *.smurfs format. :param selected_only: Decides if only models with selected root links are exported. :param subfolders: If True, the export is structured with subfolders for each model. """ objects = {} models = {} # models to be exported by name instances = [] #the instances to export for root in selectionUtils.getRoots(): if (not (selected_only and not root.select)): if "modelname" in root: objects[root['modelname']] = selectionUtils.getChildren(root) if not root['modelname'] in models: models[root['modelname']] = [root] else: models[root['modelname']].append(root) elif "reference" in root: instances.append(root["reference"]) if not root['reference'] in models: models[root['reference']] = [root] else: models[root['reference']].append(root) entities = [] for modelname in models: entitylist = models[modelname] unnamed_entities = 0 for entity in entitylist: if 'entityname' in entity: entityname = entity['entityname'] else: entityname = modelname+'_'+str(unnamed_entities) unnamed_entities += 1 entitypose = robotdictionary.deriveObjectPose(entity) uri = os.path.join(modelname, modelname+'.smurf') if subfolders else modelname+'.smurf' scenedict = {'name': entityname, 'type': 'smurf', 'file': uri, 'anchor': entity['anchor'] if 'anchor' in entity else 'none', 'position': entitypose['translation'], 'rotation': entitypose['rotation_quaternion'], 'pose': 'default'} # TODO: implement multiple poses entities.append(scenedict) if bpy.data.worlds[0].relativePath: outpath = securepath(os.path.expanduser(os.path.join(bpy.path.abspath("//"), bpy.data.worlds[0].path))) else: outpath = securepath(os.path.expanduser(bpy.data.worlds[0].path)) with open(os.path.join(outpath, bpy.data.worlds['World'].sceneName + '.smurfs'), 'w') as outputfile: outputfile.write("# SMURF scene: '" + bpy.data.worlds['World'].sceneName + "'; created " + datetime.now().strftime("%Y%m%d_%H:%M") + "\n") outputfile.write("# created with Phobos " + defs.version + " - https://github.com/rock-simulation/phobos\n\n") outputfile.write(yaml.dump({'entities': entities})) for modelname in objects: smurf_outpath = securepath(os.path.join(outpath, modelname) if subfolders else outpath) selectionUtils.selectObjects(objects[modelname], True) export(smurf_outpath) for instance in set(instances).difference(set(objects)): libpath = os.path.join(os.path.dirname(__file__), "lib") if os.path.isdir(os.path.join(outpath, instance)): shutil.rmtree(os.path.join(outpath, instance)) shutil.copytree(os.path.join(libpath, instance), os.path.join(outpath, instance))
def exportModelToSRDF(model, path): """This function exports the SRDF-relevant data from the dictionary to a specified path. Further detail on different elements of SRDF: <group> Groups in SRDF can contain *links*, *joints*, *chains* and other *groups* (the latter two of which have to be specified upstream. As nested groups is just a shortcut for adding links and joints to a group, it is not supported and the user will have to add all links and joints explicitly to each group. Originally both links and their associated parent joints were added. SRDF however implicitly assumes this, so the current implementation only adds the links. <chain> Chains are elements to simplify defining groups and are supported. The dictionary also contains a list of all elements belonging to that chain, which is discarded and not written to SRDF, however. It might be written to SMURF in the future. <link_sphere_approximatio> SRDF defines the convention that if no sphere is defined, one large sphere is assumed for that link. If one wants to have no sphere at all, it is necessary to define a sphere of radius 0. As one large sphere can be explicitly added by the user and should be if that is what he intends (WYSIWYG), we add a sphere of radius 0 by default if no sphere is specified. <passive_joint> Marks a joint as passive. <disable_collisions> Disables collisions between pairs of links to simplify collision checking and avoid collisions of parents and children at their joints. Currently not supported: - <group_state> - <virtual_joint> :param model: a robot model dictionary. :param path: the outpath for the file. :return: Nothing. """ output = [] output.append(xmlHeader) output.append(indent + '<robot name="' + model['modelname'] + '">\n\n') sorted_group_keys = get_sorted_keys(model['groups']) for groupname in sorted_group_keys: output.append(indent * 2 + '<group name="' + groupname + '">\n') # TODO: once groups are implemented, this should be sorted aswell: for member in model['groups'][groupname]: output.append(indent * 3 + '<' + member['type'] + ' name="' + member['name'] + '" />\n') output.append(indent * 2 + '</group>\n\n') sorted_chain_keys = get_sorted_keys(model['chains']) for chainname in sorted_chain_keys: output.append(indent * 2 + '<group name="' + chainname + '">\n') chain = model['chains'][chainname] output.append(indent * 3 + '<chain base_link="' + chain['start'] + '" tip_link="' + chain['end'] + '" />\n') output.append(indent * 2 + '</group>\n\n') #for joint in model['state']['joints']: # pass #passive joints sorted_joint_keys = get_sorted_keys(model['joints']) for joint in sorted_joint_keys: try: if model['joints'][joint]['passive']: output.append(indent * 2 + '<passive_joint name="' + model['links'][joint]['name'] + '"/>\n\n') except KeyError: pass sorted_link_keys = get_sorted_keys(model['links']) for link in sorted_link_keys: if len(model['links'][link]['approxcollision']) > 0: output.append(indent * 2 + '<link_sphere_approximation link="' + model['links'][link]['name'] + '">\n') # TODO: there does not seem to be a way to sort the spheres if there are multiple for sphere in model['links'][link]['approxcollision']: output.append(xmlline(3, 'sphere', ('center', 'radius'), (l2str(sphere['center']), sphere['radius']))) output.append(indent * 2 + '</link_sphere_approximation>\n\n') else: output.append(indent * 2 + '<link_sphere_approximation link="' + model['links'][link]['name'] + '">\n') output.append(xmlline(3, 'sphere', ('center', 'radius'), ('0.0 0.0 0.0', '0'))) output.append(indent * 2 + '</link_sphere_approximation>\n\n') #calculate collision-exclusive links collisionExclusives = [] for combination in itertools.combinations(model['links'], 2): link1 = model['links'][combination[0]] link2 = model['links'][combination[1]] # TODO: we might want to automatically add parent/child link combinations try: if link1['collision_bitmask'] & link2['collision_bitmask'] == 0: #output.append(xmlline(2, 'disable_collisions', ('link1', 'link2'), (link1['name'], link2['name']))) collisionExclusives.append((link1['name'], link2['name'])) except KeyError: pass 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) # FIXME: Do we need this? roots = selectionUtils.getRoots() for root in roots: if root.name == 'root': addPCCombinations(root) for pair in collisionExclusives: output.append(xmlline(2, 'disable_collisions', ('link1', 'link2'), (pair[0], pair[1]))) output.append('\n') #finish the export output.append(xmlFooter) with open(path, 'w') as outputfile: outputfile.write(''.join(output)) # FIXME: problem of different joint transformations needed for fixed joints print("phobos SRDF export: Writing model data to", path)