diff --git a/ARTools.py b/ARTools.py index 65d2a84..d9975a0 100644 --- a/ARTools.py +++ b/ARTools.py @@ -3,6 +3,7 @@ import Part import json # For exporting part infos import os # for safer path handling import GazeboExport +import GraspPose if FreeCAD.GuiUp: import FreeCADGui from PySide import QtGui @@ -199,11 +200,9 @@ def exportGazeboModels(): export_dir = QtGui.QFileDialog.getExistingDirectory(None, "Choose Export Directory", os.path.split(doc.FileName)[0]) - # Gather the unique shapes, and clone parts + # Gather the unique shapes, and clone parts as + # dict = { partX : { obj1: , graspposes: {}, placements : {}, mesh: } } unique_objs = [] - # dict for export parts = { - # partX : { obj1: , graspposes: {}, mesh: }, - # partY : { obj2: , graspposes: {}, mesh: } } parts = {} num_objs = 0 for obj in doc.Objects: @@ -226,14 +225,14 @@ def exportGazeboModels(): # if Shape is unique export mesh if new_shape: unique_objs.append(obj) - parts[obj.Label] = {"obj": obj, "graspposes": {}, "mesh": mesh_file} + parts[obj.Label] = {"obj": obj, "graspposes": {}, "placements": {}, "mesh": mesh_file} - # Add grasp poses to parts dictionary for obj in doc.Objects: + # Add grasp poses to parts dictionary if "PartToHandle" in obj.PropertiesList: graspposes = { obj.Container.Label: { "placement": placement2pose(obj.Container.Placement), - "distance": obj.GripSize + "distance": obj.GripSize*1e-3 # "OperationType" : obj.OperationType # "Operation Priority" : obj.OperationPriority # obj.Operation Parameter 1 : obj.OperationParameter1 @@ -243,32 +242,44 @@ def exportGazeboModels(): } 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) + # Add part placement position on Plane surface + import ARFrames + if hasattr(obj, 'Proxy') and "ShapeType" in obj.PropertiesList: + if isinstance(obj.Proxy, ARFrames.FeatureFrame) and obj.ShapeType == 'Face': + parts[obj.Part.Label].update({ "placements": { obj.Label: placement2pose(obj.Placement) } }) + + # Create SDF package from Parts or other packages + def create_package(name, objects, export_dir): + model_dir = os.path.join(export_dir, name) 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) + GazeboExport.export_collada(objects, parts[name]["mesh"]) + GazeboExport.export_sdf({ name: parts[name] }, export_dir, obj.Label) with open(os.path.join(model_dir, 'model.config'), 'w') as config_file: - config_file.write(GazeboExport.config(obj.Label, + config_file.write(GazeboExport.config(name, 'model.sdf', 'Author', 'Email', 'Comment', 'Version')) - if len(parts[obj.Label]["graspposes"]) > 0: - 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=(',', ': ')) + with open(os.path.join(model_dir, 'frames.json'), 'w') as frames_file: + json.dump({"label": name, + "placement": placement2pose(parts[name]["obj"].Placement), + "features": + { "graspposes" : parts[name]["graspposes"] + , "placements" : parts[name]["placements"]}}, + frames_file, indent=1, separators=(',', ': ')) + + + # Export assets for parts + for obj in selected_objects: + create_package(obj.Label, [obj], export_dir) + + # Export asset for subassembly + # subasm_name = "_".join(list(map(lambda x: x.Label[:8], selected_objects))) + # create_package(subasm_name, selected_objects, export_dir) 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): """ @@ -315,8 +326,8 @@ def exportFeatureFrames(obj, ofile): import ARFrames ff_check = lambda x: isinstance(x.Proxy, ARFrames.FeatureFrame) if hasattr(x, 'Proxy') else False ff_list = filter(ff_check, obj.InList) - ff_named = {ff.Label: ff.Proxy.getDict() for ff in ff_list} - feature_dict = {"features": ff_named} + ff_named = { ff.Label: ff.Proxy.getDict() for ff in ff_list } + feature_dict = { "features": ff_named } # File stuff odir, of = os.path.split(ofile) @@ -338,8 +349,8 @@ def appendFeatureFrames(obj, ofile): partprops = json.load(propfile) ff_check = lambda x: isinstance(x.Proxy, ARFrames.FeatureFrame) if hasattr(x, 'Proxy') else False ff_list = filter(ff_check, obj.InList) - ff_named = {ff.Label: ff.Proxy.getDict() for ff in ff_list} - feature_dict = {"features": ff_named} + ff_named = { ff.Label: {"label": ff.Label, "placement": placement2pose(ff.Placement)} for ff in ff_list } + feature_dict = { "features": ff_named } if "features" not in partprops.keys(): partprops.update(feature_dict) else: @@ -521,6 +532,12 @@ spawnClassCommand("ExportGazeboModels", "MenuText": "Export SDF-models to Gazebo", "ToolTip": "Export SDF-models for all solid parts"}) +spawnClassCommand("InsertGraspPose", + GraspPose.insert, + {"Pixmap": str(os.path.join(icondir, "addgrasppose.svg")), + "MenuText": "Insert Grasp Pose", + "ToolTip": "Insert Grasp Pose for Selected Part"}) + ################################################################### # Information from primitive type diff --git a/GraspPose.py b/GraspPose.py index 7bf01a3..bdd44ce 100644 --- a/GraspPose.py +++ b/GraspPose.py @@ -1,3 +1,9 @@ +import FreeCAD +from FreeCAD import Base, Placement +import Part +from time import sleep +import PySide + # MACRO 1: # Select part and run it to insert gripper pose (red) # Select gripper body and run it to insert second pose (pre-gripper, blue) @@ -9,10 +15,58 @@ # https://gitlab.com/robosphere/robossembler-ros2/-/blob/71938716043c35689a1058b29cefb1997a8bcf0a/rasmt_support/meshes/collision/Grip_R.STL # https://gitlab.com/robosphere/robossembler-ros2/-/blob/71938716043c35689a1058b29cefb1997a8bcf0a/rasmt_support/urdf/tools/rasmt_hand_macro.xacro +def insert(): + if len(FreeCAD.Gui.Selection.getSelection())>0: + active_body = FreeCAD.Gui.Selection.getSelection()[0] + if "IsMainPosition" in active_body.PropertiesList: + if active_body.IsMainPosition == False: + p = controlled_insert("P") + p.addProperty("App::PropertyLink", "PartToPrint", "Parameter", "Part to be printed on this table") + p.ViewObject.ShapeColor=(0.0,1.0,0.0,0.0) + p.PartToPrint = active_body.PartToHandle + p.ViewObject.Transparency = 90 + p.Placement = active_body.PartToHandle.getGlobalPlacement() + p.Label = '3D_printer_table_for_'+p.PartToPrint.Name + else: + b = grip_helper((0.0,0.0,1.0,0.0)) + b.addProperty("App::PropertyLink", "MainPosition", "Parameter", "Main position") + b.PartToHandle = active_body.PartToHandle + b.MainPosition = active_body + b.IsMainPosition = False + b.GripSize = b.MainPosition.GripSize + b.Container.Placement = b.MainPosition.Container.Placement + b.Container.Label = 'PreGripper_for_'+b.PartToHandle.Name + tempshape = Part.getShape(b.PartToHandle,'',needSubElement=False,refine=False) + FreeCAD.ActiveDocument.addObject('Part::Feature','PartToHandle').Shape = tempshape + n = FreeCAD.ActiveDocument.ActiveObject + n.Label = b.PartToHandle.Label + n.ViewObject.ShapeColor = (0.0,0.0,1.0,0.0) + n.adjustRelativeLinks(b.Container) + b.Container.addObject(n) + n.Placement.Base = n.getGlobalPlacement().Base.sub(b.Container.Placement) + n.Placement.Rotation.Axis = n.getGlobalPlacement().Rotation.Axis.sub(b.Container.Placement.Rotation.Axis) + n.Placement.Rotation.Angle = n.getGlobalPlacement().Rotation.Angle-b.Container.Placement.Rotation.Angle + n.ViewObject.ShowInTree = False + n.ViewObject.Transparency = 90 + + else: + b = grip_helper((1.0,0.0,0.0,0.0)) + b.addProperty("App::PropertyInteger", "OperationPriority", "Parameter", "Priority of the operation") + b.addProperty("App::PropertyInteger", "OperationType", "Parameter", "Priority of the operation") + b.addProperty("App::PropertyFloat", "OperationParameter1", "Parameter", "Priority of the operation") + b.addProperty("App::PropertyFloat", "OperationParameter2", "Parameter", "Priority of the operation") + b.addProperty("App::PropertyFloat", "OperationParameter3", "Parameter", "Priority of the operation") + b.PartToHandle = active_body + b.IsMainPosition = True + b.GripSize = active_body.Shape.BoundBox.YLength + b.Container.Placement = active_body.getGlobalPlacement() + b.Container.Label = 'Gripper_for_'+b.PartToHandle.Name + + def controlled_insert(code): - a=App.ActiveDocument.Objects - Part.insert(u"C:/Users/ibryl/AppData/Roaming/FreeCAD/Mod/ARBench/"+code+".brep",App.ActiveDocument.Name) - b=App.ActiveDocument.Objects + a = FreeCAD.ActiveDocument.Objects + Part.insert(u"C:/Users/ibryl/AppData/Roaming/FreeCAD/Mod/ARBench/"+code+".brep",FreeCAD.ActiveDocument.Name) + b = FreeCAD.ActiveDocument.Objects return list(set(b) - set(a))[0] def grip_helper(color): @@ -25,147 +79,90 @@ def grip_helper(color): b.addProperty("App::PropertyLink", "PartToHandle", "Parameter", "Part to be manipulated by this gripper") r.setExpression('.Placement.Base.y', b.Name+'.GripSize / 2') l.setExpression('.Placement.Base.y', '-'+b.Name+'.GripSize / 2') - b.ViewObject.ShapeColor=color - r.ViewObject.ShapeColor=color - l.ViewObject.ShapeColor=color - NewPart = App.activeDocument().addObject('App::Part','Part') + b.ViewObject.ShapeColor = color + r.ViewObject.ShapeColor = color + l.ViewObject.ShapeColor = color + NewPart = FreeCAD.activeDocument().addObject('App::Part','Part') b.adjustRelativeLinks(NewPart) NewPart.addObject(b) r.adjustRelativeLinks(NewPart) NewPart.addObject(r) l.adjustRelativeLinks(NewPart) NewPart.addObject(l) - r.ViewObject.ShowInTree=False - l.ViewObject.ShowInTree=False - b.Container=NewPart - b.ViewObject.Transparency=90 - r.ViewObject.Transparency=90 - l.ViewObject.Transparency=90 + r.ViewObject.ShowInTree = False + l.ViewObject.ShowInTree = False + b.Container = NewPart + b.ViewObject.Transparency = 90 + r.ViewObject.Transparency = 90 + l.ViewObject.Transparency = 90 return b - -if len(Gui.Selection.getSelection())>0: - active_body=Gui.Selection.getSelection()[0] - if "IsMainPosition" in active_body.PropertiesList: - if active_body.IsMainPosition == False: - p=controlled_insert("P") - p.addProperty("App::PropertyLink", "PartToPrint", "Parameter", "Part to be printed on this table") - p.ViewObject.ShapeColor=(0.0,1.0,0.0,0.0) - p.PartToPrint=active_body.PartToHandle - p.ViewObject.Transparency=90 - p.Placement=active_body.PartToHandle.getGlobalPlacement() - p.Label = '3D_printer_table_for_'+p.PartToPrint.Name - else: - b=grip_helper((0.0,0.0,1.0,0.0)) - b.addProperty("App::PropertyLink", "MainPosition", "Parameter", "Main position") - b.PartToHandle=active_body.PartToHandle - b.MainPosition=active_body - b.IsMainPosition=False - b.GripSize=b.MainPosition.GripSize - b.Container.Placement=b.MainPosition.Container.Placement - b.Container.Label = 'PreGripper_for_'+b.PartToHandle.Name - tempshape = Part.getShape(b.PartToHandle,'',needSubElement=False,refine=False) - App.ActiveDocument.addObject('Part::Feature','PartToHandle').Shape=tempshape - n=App.ActiveDocument.ActiveObject - n.Label=b.PartToHandle.Label - n.ViewObject.ShapeColor=(0.0,0.0,1.0,0.0) - n.adjustRelativeLinks(b.Container) - b.Container.addObject(n) - n.Placement.Base = n.getGlobalPlacement().Base.sub(b.Container.Placement) - n.Placement.Rotation.Axis = n.getGlobalPlacement().Rotation.Axis.sub(b.Container.Placement.Rotation.Axis) - n.Placement.Rotation.Angle = n.getGlobalPlacement().Rotation.Angle-b.Container.Placement.Rotation.Angle - n.ViewObject.ShowInTree=False - n.ViewObject.Transparency=90 - - else: - b=grip_helper((1.0,0.0,0.0,0.0)) - b.addProperty("App::PropertyInteger", "OperationPriority", "Parameter", "Priority of the operation") - b.addProperty("App::PropertyInteger", "OperationType", "Parameter", "Priority of the operation") - b.addProperty("App::PropertyFloat", "OperationParameter1", "Parameter", "Priority of the operation") - b.addProperty("App::PropertyFloat", "OperationParameter2", "Parameter", "Priority of the operation") - b.addProperty("App::PropertyFloat", "OperationParameter3", "Parameter", "Priority of the operation") - b.PartToHandle=active_body - b.IsMainPosition=True - b.GripSize=active_body.Shape.BoundBox.YLength - b.Container.Placement=active_body.getGlobalPlacement() - b.Container.Label = 'Gripper_for_'+b.PartToHandle.Name - # MACRO 3: # Select pre-gripper body and run it to animate its movement +def select_pregripper(): + apart = FreeCAD.Gui.Selection.getSelection()[0] + sp = apart.Container.Placement.Base + ep = apart.MainPosition.Container.Placement.Base + sa = apart.Container.Placement.Rotation.Angle + ea = apart.MainPosition.Container.Placement.Rotation.Angle + print (sp,ep) -from FreeCAD import Base,Placement -import Part -from time import sleep -import PySide + i = 0.0 + def updatePlacement(): + global timer + global i + global sp + global ep + global sa + global ea + apart.Container.Placement.Base = ep.multiply(i).add(sp.multiply(1.0-i)) + apart.Container.Placement.Rotation.Angle = (ea*i)+(sa*(1.0-i)) + i += 0.02 + if i>=1: + #apart.Container.Placement.Base=sp + #apart.Container.Placement.Rotation.Angle=sa + timer.stop() + FreeCAD.Gui.updateGui() -apart=Gui.Selection.getSelection()[0] -sp=apart.Container.Placement.Base -ep=apart.MainPosition.Container.Placement.Base -sa=apart.Container.Placement.Rotation.Angle -ea=apart.MainPosition.Container.Placement.Rotation.Angle -print (sp,ep) - -i=0.0 -def updatePlacement(): - global timer - global i - global sp - global ep - global sa - global ea - apart.Container.Placement.Base=ep.multiply(i).add(sp.multiply(1.0-i)) - apart.Container.Placement.Rotation.Angle=(ea*i)+(sa*(1.0-i)) - i+=0.02 - if i>=1: - #apart.Container.Placement.Base=sp - #apart.Container.Placement.Rotation.Angle=sa - timer.stop() - FreeCAD.Gui.updateGui() - -timer = PySide.QtCore.QTimer() -timer.timeout.connect(updatePlacement) -timer.start(100) + timer = PySide.QtCore.QTimer() + timer.timeout.connect(updatePlacement) + timer.start(100) # MACRO 4: #Raw advanced version of MACRO 1 #Does not work properly yet -def controlled_insert(code): - a=App.ActiveDocument.Objects - Part.insert(u"C:/Users/MariaR/Desktop/"+code+".brep",App.ActiveDocument.Name) - b=App.ActiveDocument.Objects - return list(set(b) - set(a))[0] - -if len(Gui.Selection.getSelection())>0: - active_body=Gui.Selection.getSelection()[0] - b = controlled_insert("B") - r = controlled_insert("R") - l = controlled_insert("L") - b.ViewObject.ShapeColor=(1.0,0.0,0.0,0.0) - r.ViewObject.ShapeColor=(1.0,0.0,0.0,0.0) - l.ViewObject.ShapeColor=(1.0,0.0,0.0,0.0) - b.ViewObject.ShowInTree=False - r.ViewObject.ShowInTree=False - l.ViewObject.ShowInTree=False - b.ViewObject.Transparency=90 - r.ViewObject.Transparency=90 - l.ViewObject.Transparency=90 - a=FreeCAD.ActiveDocument.addObject("App::FeaturePython",'Gripper_for_'+active_body.Name) - a.addProperty("App::PropertyFloat", "GripSize", "Parameter", "Size between fingers") - a.addProperty("App::PropertyLink", "PartToHandle", "Parameter", "Part to be manipulated by this gripper") - a.addProperty("App::PropertyLink", "GripperBody", "Parameter", "Body") - a.addProperty("App::PropertyLink", "GripperLF", "Parameter", "Body") - a.addProperty("App::PropertyLink", "GripperRF", "Parameter", "Body") - a.GripperBody=b - a.GripperLF=l - a.GripperRF=r - #r.setExpression('.Placement.Base.y', a.Name+'.GripSize / 2') - #l.setExpression('.Placement.Base.y', '-'+a.Name+'.GripSize / 2') - a.addProperty("App::PropertyInteger", "OperationPriority", "Parameter", "Priority of the operation") - a.addProperty("App::PropertyInteger", "OperationType", "Parameter", "Priority of the operation") - a.addProperty("App::PropertyFloat", "OperationParameter1", "Parameter", "Priority of the operation") - a.addProperty("App::PropertyFloat", "OperationParameter2", "Parameter", "Priority of the operation") - a.addProperty("App::PropertyFloat", "OperationParameter3", "Parameter", "Priority of the operation") - a.PartToHandle=active_body - a.GripSize=active_body.Shape.BoundBox.YLength +def insert_advanced(): + if len(FreeCAD.Gui.Selection.getSelection())>0: + active_body = FreeCAD.Gui.Selection.getSelection()[0] + b = controlled_insert("B") + r = controlled_insert("R") + l = controlled_insert("L") + b.ViewObject.ShapeColor = (1.0,0.0,0.0,0.0) + r.ViewObject.ShapeColor = (1.0,0.0,0.0,0.0) + l.ViewObject.ShapeColor = (1.0,0.0,0.0,0.0) + b.ViewObject.ShowInTree = False + r.ViewObject.ShowInTree = False + l.ViewObject.ShowInTree = False + b.ViewObject.Transparency = 90 + r.ViewObject.Transparency = 90 + l.ViewObject.Transparency = 90 + a = FreeCAD.ActiveDocument.addObject("App::FeaturePython",'Gripper_for_'+active_body.Name) + a.addProperty("App::PropertyFloat", "GripSize", "Parameter", "Size between fingers") + a.addProperty("App::PropertyLink", "PartToHandle", "Parameter", "Part to be manipulated by this gripper") + a.addProperty("App::PropertyLink", "GripperBody", "Parameter", "Body") + a.addProperty("App::PropertyLink", "GripperLF", "Parameter", "Body") + a.addProperty("App::PropertyLink", "GripperRF", "Parameter", "Body") + a.GripperBody = b + a.GripperLF = l + a.GripperRF = r + #r.setExpression('.Placement.Base.y', a.Name+'.GripSize / 2') + #l.setExpression('.Placement.Base.y', '-'+a.Name+'.GripSize / 2') + a.addProperty("App::PropertyInteger", "OperationPriority", "Parameter", "Priority of the operation") + a.addProperty("App::PropertyInteger", "OperationType", "Parameter", "Priority of the operation") + a.addProperty("App::PropertyFloat", "OperationParameter1", "Parameter", "Priority of the operation") + a.addProperty("App::PropertyFloat", "OperationParameter2", "Parameter", "Priority of the operation") + a.addProperty("App::PropertyFloat", "OperationParameter3", "Parameter", "Priority of the operation") + a.PartToHandle = active_body + a.GripSize = active_body.Shape.BoundBox.YLength diff --git a/InitGui.py b/InitGui.py index e1d664e..cfa408e 100644 --- a/InitGui.py +++ b/InitGui.py @@ -15,7 +15,8 @@ class ARBench(Workbench): "AllPartFramesCommand", "FeatureFrameCommand"] self.toolcommands = ["ExportPartInfoAndFeaturesDialogueCommand", - "ExportGazeboModels"] + "ExportGazeboModels", + "InsertGraspPose"] self.appendToolbar("AR Frames", self.framecommands) self.appendToolbar("AR Tools", self.toolcommands) diff --git a/UI/icons/addgrasppose.svg b/UI/icons/addgrasppose.svg new file mode 100644 index 0000000..3c74388 --- /dev/null +++ b/UI/icons/addgrasppose.svg @@ -0,0 +1,16 @@ + + + +Created with Fabric.js 4.6.0 + + + + + + + + + + + + \ No newline at end of file