Export Grasp poses in json; export mesh once for same parts

This commit is contained in:
Igor Brylyov 2022-03-02 23:49:29 +03:00
parent 7f6e9c98aa
commit 20900e4eba
3 changed files with 124 additions and 93 deletions

View file

@ -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,12 +173,65 @@ def getLocalPartProps(obj):
obj.Placement = old_placement
return partprops
# if "IsMainPosition" in obj.PropertiesList:
# Longest match for mesh name
def getGraspPoseProps(obj):
# part = obj.PartToHandle
partprops = getLocalPartProps(obj.PartToHandle)
grasppose = { obj.Container.Label: {
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
@ -185,33 +239,36 @@ def getGraspPoseProps(obj):
# 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)
parts[obj.PartToHandle.Label].update({"graspposes" : graspposes})
opts = QtGui.QFileDialog.DontConfirmOverwrite
# 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)
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"
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))
with open(ofile, "w", encoding="utf8") as propfile:
json.dump(partprops, propfile, indent=1, separators=(',', ': '))
###################################################################
# Export functions
###################################################################
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

View file

@ -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,44 +27,30 @@ 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
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)
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')
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)
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)
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)
visual = Visual(name=name+'_visual', mesh=mesh_uri)
collision = Collision(name=name+'_collision', mesh=mesh_uri)
link = Link(name=name,
link = Link(name=label,
pose=pose,
inertial=inertial,
visual=visual,
@ -72,9 +60,6 @@ def export_gazebo_model(obj, export_dir, configs={}):
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
###################################################################

View file

@ -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]