robossembler/framework
7
0
Fork 0
Code Issues 21 Pull requests Projects Releases Packages Wiki Activity Actions

Grasp Pose json export function moved to ARTools module; export format adopted to geometry_msgs/Pose

Browse source
This commit is contained in:
Igor Brylyov 2022-02-22 20:45:54 +03:00
parent 3941c7aae6
commit 7f6e9c98aa
2 changed files with 62 additions and 57 deletions
Download patch file Download diff file Expand all files Collapse all files

60
ARTools.py
View file

@ -31,6 +31,20 @@ def matrix2list(mat, scale=1e-3):
[mat.A31, mat.A32, mat.A33, mat.A34*scale],
[mat.A41, mat.A42, mat.A43, mat.A44]]
def placement2pose(pl, scale=1e-3):
"""Gives the placement as an dictionary for geometry_msgs/Pose type."""
return {"position": {
"x": pl.Base.x*scale,
"y": pl.Base.y*scale,
"z": pl.Base.z*scale
},
"orientation": {
"x": pl.Rotation.Axis.x,
"y": pl.Rotation.Axis.y,
"z": pl.Rotation.Axis.z,
"w": pl.Rotation.Angle
}
}
def placement2axisvec(pl, scale=1e-3):
"""Gives the placement as an dictionary of origin and rotation.
@ -149,15 +163,51 @@ def getLocalPartProps(obj):
# Part properties
partprops = {
"label": obj.Label,
"placement": placement2axisvec(old_placement),
"boundingbox": boundingBox2list(obj.Shape.BoundBox),
"volume": obj.Shape.Volume*1e-9,
"centerofmass": vector2list(obj.Shape.CenterOfMass),
"principalproperties": principalProperties2dict(obj.Shape.PrincipalProperties)
"placement": placement2pose(old_placement),
# "boundingbox": boundingBox2list(obj.Shape.BoundBox),
# "volume": obj.Shape.Volume*1e-9,
# "centerofmass": vector2list(obj.Shape.CenterOfMass),
# "principalproperties": principalProperties2dict(obj.Shape.PrincipalProperties)
}
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=(',', ': '))
###################################################################
# Export functions

59
GraspPose.py
View file

@ -1,7 +1,13 @@
# 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)
# Select pre-gripper body and run it to insert 3D pronter table (green)
# Select pre-gripper body and run it to insert 3D printer table (green)
# Current gripper
# https://gitlab.com/robosphere/robossembler-ros2/-/blob/71938716043c35689a1058b29cefb1997a8bcf0a/rasmt_support/meshes/collision/Grip_Body.STL
# https://gitlab.com/robosphere/robossembler-ros2/-/blob/71938716043c35689a1058b29cefb1997a8bcf0a/rasmt_support/meshes/collision/Grip_L.STL
# 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 controlled_insert(code):
a=App.ActiveDocument.Objects
@ -84,57 +90,6 @@ if len(Gui.Selection.getSelection())>0:
b.Container.Placement=active_body.getGlobalPlacement()
b.Container.Label = 'Gripper_for_'+b.PartToHandle.Name
# MACRO 2:
#Exports all grasp poses to json file
import json # For exporting part infos
import os # for safer path handling
if FreeCAD.GuiUp:
import FreeCADGui
from PySide import QtGui
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"
mp={}
for a in App.ActiveDocument.Objects:
if "IsMainPosition" in a.PropertiesList:
pp={}
pp['Part name'] = a.PartToHandle.Name
pp['Part label'] = a.PartToHandle.Label
papb=a.PartToHandle.Placement.Base
pp['Part position XYZ'] = [papb.x,papb.y,papb.z]
papa=a.PartToHandle.Placement.Rotation.Axis
pp['Part rotation axis XYZ'] = [papa.x,papa.y,papa.z]
pp['Part rotation angle'] = a.PartToHandle.Placement.Rotation.Angle
apb=a.Container.Placement.Base
pp['Gripper position XYZ'] = [apb.x,apb.y,apb.z]
apa=a.Container.Placement.Rotation.Axis
pp['Gripper rotation axis XYZ'] = [apa.x,apa.y,apa.z]
pp['Gripper rotation angle'] = a.Container.Placement.Rotation.Angle
pp['Grip size'] = a.GripSize
pp['OperationType'] = a.OperationType
pp['Operation Priority'] = a.OperationPriority
pp['a.Operation Parameter 1'] = a.OperationParameter1
pp['a.Operation Parameter 2'] = a.OperationParameter2
pp['a.Operation Parameter 3'] = a.OperationParameter3
mp[a.Container.Label]=pp
with open(ofile, "w", encoding="utf8") as propfile:
json.dump(mp, propfile, indent=1, separators=(',', ': '))
# MACRO 3:
# Select pre-gripper body and run it to animate its movement