diff --git a/ARTools.py b/ARTools.py index 3a1eb26..b6cf099 100644 --- a/ARTools.py +++ b/ARTools.py @@ -2,6 +2,7 @@ import FreeCAD import Part import json # For exporting part infos import os # for safer path handling +import GazeboExport if FreeCAD.GuiUp: import FreeCADGui from PySide import QtGui @@ -172,46 +173,102 @@ def getLocalPartProps(obj): obj.Placement = old_placement return partprops -# if "IsMainPosition" in obj.PropertiesList: - -def getGraspPoseProps(obj): - # part = obj.PartToHandle - partprops = getLocalPartProps(obj.PartToHandle) - grasppose = { obj.Container.Label: { - "placement": placement2pose(obj.Container.Placement), - "distance": obj.GripSize - # "OperationType" : obj.OperationType - # "Operation Priority" : obj.OperationPriority - # obj.Operation Parameter 1 : obj.OperationParameter1 - # obj.Operation Parameter 2 : obj.OperationParameter2 - # obj.Operation Parameter 3 : obj.OperationParameter3 - - } - } - graspposes = {"features": {"grasp-poses": grasppose }} - partprops.update(graspposes) - - opts = QtGui.QFileDialog.DontConfirmOverwrite - - ofile, filt = QtGui.QFileDialog.getSaveFileName(None, 'test', - os.getenv("HOME"), - "*.json", options=opts) - odir, of = os.path.split(ofile) - if not os.path.exists(odir): - os.makedirs(odir) - if not of.lower().endswith(".json"): - ofile = ofile + ".json" - - - with open(ofile, "w", encoding="utf8") as propfile: - json.dump(partprops, propfile, indent=1, separators=(',', ': ')) +# Longest match for mesh name +def longest_match(seq1, seq2): + from difflib import SequenceMatcher as SM + sm = SM(lambda c: c in set(' ,'), seq1, seq2) + m = sm.find_longest_match(0, len(seq1), 0, len(seq2)) + return seq1[m.a:m.b] ################################################################### # Export functions ################################################################### + +def exportGazeboModels(): + """Export packages for Gazebo Simulator.""" + doc = FreeCAD.activeDocument() + selected_objects = FreeCADGui.Selection.getSelection() + FreeCADGui.Selection.clearSelection() + if len(selected_objects) == 0: + FreeCAD.Console.PrintError("No part selected.") + return False + + export_dir = QtGui.QFileDialog.getExistingDirectory(None, "Choose Export Directory", + os.path.split(doc.FileName)[0]) + + # Gather the unique shapes, and clone parts + unique_objs = [] + # dict for export parts = { + # partX : { obj1: , graspposes: {}, mesh: }, + # partY : { obj2: , graspposes: {}, mesh: } } + parts = {} + num_objs = 0 + for obj in doc.Objects: + new_shape = True + model_dir = os.path.join(export_dir, obj.Label) + mesh_dir = os.path.join(model_dir, 'meshes') + mesh_file = os.path.join(mesh_dir, obj.Label + '.dae') + mesh_uri = os.path.normpath(os.path.relpath(mesh_file, export_dir)) + # Select only Parts, not Grasp Poses or Gripper + if obj.TypeId == "Part::Feature" and not "PartToHandle" in obj.PropertiesList and not "Container" in obj.PropertiesList: + num_objs += 1 + for uobj in unique_objs: + if uobj.Shape.isPartner(obj.Shape): + new_shape = False + # parts[obj.Label]["mesh"] += parts[uobj.Label]["mesh"] + parts[obj.Label] = {"obj": obj, + "graspposes": {}, + "mesh": parts[uobj.Label]["mesh"] + } + # if Shape is unique export mesh + if new_shape: + unique_objs.append(obj) + parts[obj.Label] = {"obj": obj, "graspposes": {}, "mesh": mesh_file} + + # Add grasp poses to parts dictionary + for obj in doc.Objects: + if "PartToHandle" in obj.PropertiesList: + graspposes = { obj.Container.Label: { + "placement": placement2pose(obj.Container.Placement), + "distance": obj.GripSize + # "OperationType" : obj.OperationType + # "Operation Priority" : obj.OperationPriority + # obj.Operation Parameter 1 : obj.OperationParameter1 + # obj.Operation Parameter 2 : obj.OperationParameter2 + # obj.Operation Parameter 3 : obj.OperationParameter3 + } + } + parts[obj.PartToHandle.Label].update({"graspposes" : graspposes}) + + # Export assets for selected objects + for obj in selected_objects: + model_dir = os.path.join(export_dir, obj.Label) + mesh_dir = os.path.join(model_dir, 'meshes') + os.makedirs(mesh_dir, exist_ok=True) + + GazeboExport.export_collada([obj], parts[obj.Label]["mesh"]) + GazeboExport.export_sdf({ obj.Label: parts[obj.Label] }, export_dir, obj.Label) + + with open(os.path.join(model_dir, 'model.config'), 'w') as config_file: + config_file.write(GazeboExport.config(obj.Label, + 'model.sdf', 'Author', 'Email', 'Comment', 'Version')) + + with open(os.path.join(model_dir, 'frames.json'), 'w') as frames_file: + # frames_file.write(json.dumps(parts[obj.Label]["graspposes"])) + json.dump({"features": { "grasp-poses" : parts[obj.Label]["graspposes"]}}, + frames_file, indent=1, separators=(',', ': ')) + + return True + + # if (isinstance(obj.Shape, Part.Solid) if hasattr(obj, 'Shape') else False): + + # elif isinstance(obj, Part.Feature): + # FreeCAD.Console.PrintMessage('{0} part is not valid. It has a Compound type, but Solids there are hidden. Please convert it to single Solid'.format(obj.Label)) + + def exportPartInfo(obj, ofile): """ Exports part info to a new json file. @@ -362,8 +419,7 @@ def exportFeatureFramesDialogue(): textprompt = textprompt + "s" opts = QtGui.QFileDialog.DontConfirmOverwrite # Create file dialog - ofile, filt = QtGui.QFileDialog.getSaveFileName(None, textprompt, - os.getenv("HOME"), + ofile, filt = QtGui.QFileDialog.getSaveFileName(None, textprompt, os.getenv("HOME"), "*.json", options=opts) if ofile == "": # User cancelled @@ -444,16 +500,6 @@ def exportPartInfoAndFeaturesDialogue(): + str(unique_selected[0].Label) + " exported to " + str(ofile) + "\n") -def exportGazeboModels(): - import GazeboExport - doc = FreeCAD.activeDocument() - for obj in doc.Objects: - """Export solid shapes.""" - if (isinstance(obj.Shape, Part.Solid) if hasattr(obj, 'Shape') else False): - GazeboExport.export_gazebo_model(obj, os.path.split(doc.FileName)[0], configs={}) - elif isinstance(obj, Part.Feature): - FreeCAD.Console.PrintMessage('{0} part is not valid. It has a Compound type, but Solids there are hidden. Please convert it to single Solid'.format(obj.Label)) - ################################################################### # GUI Commands diff --git a/GazeboExport.py b/GazeboExport.py index e33680e..1fc80c7 100644 --- a/GazeboExport.py +++ b/GazeboExport.py @@ -7,17 +7,19 @@ from xml.dom.minidom import parseString from math import radians as _radians import Part -def export_gazebo_model(obj, export_dir, configs={}): - name = obj.Label - +# Takes subassembly or parts dictionary { part_label: { "obj": , "mesh": } } +# and generate SDF for them + +def export_sdf(objects, export_dir, modelname, configs={}): + model_dir = os.path.join(export_dir, modelname) + scale = configs.get('scale', 0.001) scale_vec = FreeCAD.Vector([scale]*3) - density = configs.get('density', 1000) - bounding_box = obj.Shape.BoundBox + shapes = list(map(lambda x: x["obj"].Shape, objects.values())) + bounding_box = Part.makeCompound(shapes).BoundBox bounding_box.scale(*scale_vec) - global_pose_base = FreeCAD.Vector(bounding_box.XLength/2, bounding_box.YLength/2, bounding_box.ZLength/2) @@ -25,56 +27,39 @@ def export_gazebo_model(obj, export_dir, configs={}): global_pose = FreeCAD.Placement() global_pose.Base = global_pose_base - model = Model(name=name, pose=global_pose) + model = Model(name=modelname, pose=global_pose) model.self_collide = False model.sdf_version = '1.5' - shape = obj.Shape - mass = shape.Mass * scale**3 * density - com = shape.CenterOfMass * scale - inr = shape.MatrixOfInertia - inr.scale(*scale_vec*(scale**4) * density) - placement = shape.Placement - placement.Base.scale(*scale_vec) + for label in objects.keys(): + shape = objects[label]["obj"].Shape + mass = shape.Mass * scale**3 * density + com = shape.CenterOfMass * scale + inr = shape.MatrixOfInertia + inr.scale(*scale_vec*(scale**4) * density) + placement = shape.Placement + placement.Base.scale(*scale_vec) + pose = placement.copy() + pose.Base = com + pose_rpy = pose.copy() + pose_rpy.Base=(np.zeros(3)) + inertia = Inertia(inertia=np.array(inr.A)[[0,1,2,5,6,10]]) + inertial = Inertial(pose=pose_rpy, mass=mass, inertia=inertia) - model_dir = os.path.join(export_dir, name) - mesh_dir = os.path.join(model_dir, 'meshes') - mesh_file = os.path.join(mesh_dir, name + '.dae') + mesh_uri = os.path.normpath(os.path.relpath(objects[label]["mesh"], export_dir)) + visual = Visual(name=label+'_visual', mesh=mesh_uri) + collision = Collision(name=label+'_collision', mesh=mesh_uri) - os.makedirs(mesh_dir, exist_ok=True) - export_collada([obj], mesh_file, scale=scale, offset=com*-1) - - pose = placement.copy() - pose.Base = com - - pose_rpy = pose.copy() - pose_rpy.Base=(np.zeros(3)) - - - inertia = Inertia(inertia=np.array(inr.A)[[0,1,2,5,6,10]]) - inertial = Inertial(pose=pose_rpy, - mass=mass, - inertia=inertia) - - mesh_uri = os.path.relpath(mesh_file, export_dir) - mesh_uri = os.path.normpath(mesh_uri) - - visual = Visual(name=name+'_visual', mesh=mesh_uri) - collision = Collision(name=name+'_collision', mesh=mesh_uri) - - link = Link(name=name, - pose=pose, - inertial=inertial, - visual=visual, - collision=collision) - model.links.append(link) + link = Link(name=label, + pose=pose, + inertial=inertial, + visual=visual, + collision=collision) + model.links.append(link) with open(os.path.join(model_dir, 'model.sdf'), 'w') as sdf_file: sdf_file.write(model.to_xml_string('sdf')) - with open(os.path.join(model_dir, 'model.config'), 'w') as config_file: - config_file.write(config(name, 'model.sdf', 'Author', 'Email', 'Comment', 'Version')) - ################################################################### # Export helpers ################################################################### diff --git a/GraspPose.py b/GraspPose.py index 275ecec..7bf01a3 100644 --- a/GraspPose.py +++ b/GraspPose.py @@ -11,7 +11,7 @@ def controlled_insert(code): a=App.ActiveDocument.Objects - Part.insert(u"C:/Users/MariaR/Desktop/"+code+".brep",App.ActiveDocument.Name) + Part.insert(u"C:/Users/ibryl/AppData/Roaming/FreeCAD/Mod/ARBench/"+code+".brep",App.ActiveDocument.Name) b=App.ActiveDocument.Objects return list(set(b) - set(a))[0]