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

ARTools.py

@@ -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),
+        "placement": placement2pose(old_placement),
-        "boundingbox": boundingBox2list(obj.Shape.BoundBox),
+        # "boundingbox": boundingBox2list(obj.Shape.BoundBox),
-        "volume": obj.Shape.Volume*1e-9,
+        # "volume": obj.Shape.Volume*1e-9,
-        "centerofmass": vector2list(obj.Shape.CenterOfMass),
+        # "centerofmass": vector2list(obj.Shape.CenterOfMass),
-        "principalproperties": principalProperties2dict(obj.Shape.PrincipalProperties)
+        # "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

GraspPose.py

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