Export Grasp poses in json; export mesh once for same parts
This commit is contained in:
parent
7f6e9c98aa
commit
20900e4eba
3 changed files with 124 additions and 93 deletions
136
ARTools.py
136
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: <obj>, graspposes: {}, mesh: <mesh_uri> },
|
||||
# partY : { obj2: <obj>, graspposes: {}, mesh: <mesh_uri> } }
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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": <obj>, "mesh": <meshuri> } }
|
||||
# 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
|
||||
###################################################################
|
||||
|
|
|
|||
|
|
@ -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]
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue