Grasp Pose Export
This commit is contained in:
Alexander Olikevich
parent
8241627436
commit
3941c7aae6
7 changed files with 13312 additions and 0 deletions
216
GraspPose.py
Normal file
216
GraspPose.py
Normal file
|
|
@ -0,0 +1,216 @@
|
||||||
|
# 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)
|
||||||
|
|
||||||
|
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]
|
||||||
|
|
||||||
|
def grip_helper(color):
|
||||||
|
b = controlled_insert("B")
|
||||||
|
r = controlled_insert("R")
|
||||||
|
l = controlled_insert("L")
|
||||||
|
b.addProperty("App::PropertyFloat", "GripSize", "Parameter", "Size between fingers")
|
||||||
|
b.addProperty("App::PropertyLink", "Container", "Parameter", "Part Container")
|
||||||
|
b.addProperty("App::PropertyBool", "IsMainPosition", "Parameter", "Is it main or supportive position")
|
||||||
|
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.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
|
||||||
|
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 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
|
||||||
|
|
||||||
|
from FreeCAD import Base,Placement
|
||||||
|
import Part
|
||||||
|
from time import sleep
|
||||||
|
import PySide
|
||||||
|
|
||||||
|
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)
|
||||||
|
|
||||||
|
# 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
|
||||||
|
|
||||||
130
example.json
Normal file
130
example.json
Normal file
|
|
@ -0,0 +1,130 @@
|
||||||
|
{
|
||||||
|
"Gripper_for_Box": {
|
||||||
|
"Part name": "Box",
|
||||||
|
"Part label": "\u041a\u0443\u0431",
|
||||||
|
"Part position XYZ": [
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
0.0
|
||||||
|
],
|
||||||
|
"Part rotation axis XYZ": [
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
1.0
|
||||||
|
],
|
||||||
|
"Part rotation angle": 0.0,
|
||||||
|
"Gripper position XYZ": [
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
0.0
|
||||||
|
],
|
||||||
|
"Gripper rotation axis XYZ": [
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
1.0
|
||||||
|
],
|
||||||
|
"Gripper rotation angle": 0.0,
|
||||||
|
"Grip size": 10.0,
|
||||||
|
"OperationType": 0,
|
||||||
|
"Operation Priority": 0,
|
||||||
|
"a.Operation Parameter 1": 0.0,
|
||||||
|
"a.Operation Parameter 2": 0.0,
|
||||||
|
"a.Operation Parameter 3": 0.0
|
||||||
|
},
|
||||||
|
"Gripper_for_Cylinder": {
|
||||||
|
"Part name": "Cylinder",
|
||||||
|
"Part label": "\u0426\u0438\u043b\u0438\u043d\u0434\u0440",
|
||||||
|
"Part position XYZ": [
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
0.0
|
||||||
|
],
|
||||||
|
"Part rotation axis XYZ": [
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
1.0
|
||||||
|
],
|
||||||
|
"Part rotation angle": 0.0,
|
||||||
|
"Gripper position XYZ": [
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
0.0
|
||||||
|
],
|
||||||
|
"Gripper rotation axis XYZ": [
|
||||||
|
1.0,
|
||||||
|
0.0,
|
||||||
|
0.0
|
||||||
|
],
|
||||||
|
"Gripper rotation angle": 4.1887902047863905,
|
||||||
|
"Grip size": 3.9708354963922163,
|
||||||
|
"OperationType": 0,
|
||||||
|
"Operation Priority": 0,
|
||||||
|
"a.Operation Parameter 1": 0.0,
|
||||||
|
"a.Operation Parameter 2": 0.0,
|
||||||
|
"a.Operation Parameter 3": 0.0
|
||||||
|
},
|
||||||
|
"Gripper_for_Box001": {
|
||||||
|
"Part name": "Box001",
|
||||||
|
"Part label": "\u041a\u0443\u0431001",
|
||||||
|
"Part position XYZ": [
|
||||||
|
0.0,
|
||||||
|
9.0,
|
||||||
|
0.0
|
||||||
|
],
|
||||||
|
"Part rotation axis XYZ": [
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
1.0
|
||||||
|
],
|
||||||
|
"Part rotation angle": 0.0,
|
||||||
|
"Gripper position XYZ": [
|
||||||
|
3.8035643556662895,
|
||||||
|
99.89718356183919,
|
||||||
|
-9.331967349091768
|
||||||
|
],
|
||||||
|
"Gripper rotation axis XYZ": [
|
||||||
|
-0.6901619708141958,
|
||||||
|
0.717463959241293,
|
||||||
|
0.09445592216305763
|
||||||
|
],
|
||||||
|
"Gripper rotation angle": 3.570126372396101,
|
||||||
|
"Grip size": 20.0,
|
||||||
|
"OperationType": 0,
|
||||||
|
"Operation Priority": 0,
|
||||||
|
"a.Operation Parameter 1": 0.0,
|
||||||
|
"a.Operation Parameter 2": 0.0,
|
||||||
|
"a.Operation Parameter 3": 0.0
|
||||||
|
},
|
||||||
|
"Gripper_for_Tube": {
|
||||||
|
"Part name": "Tube",
|
||||||
|
"Part label": "Tube",
|
||||||
|
"Part position XYZ": [
|
||||||
|
0.0,
|
||||||
|
370.0,
|
||||||
|
10.0
|
||||||
|
],
|
||||||
|
"Part rotation axis XYZ": [
|
||||||
|
1.0,
|
||||||
|
0.0,
|
||||||
|
0.0
|
||||||
|
],
|
||||||
|
"Part rotation angle": 1.5707963267948966,
|
||||||
|
"Gripper position XYZ": [
|
||||||
|
0.0,
|
||||||
|
345.0,
|
||||||
|
10.0
|
||||||
|
],
|
||||||
|
"Gripper rotation axis XYZ": [
|
||||||
|
1.0,
|
||||||
|
0.0,
|
||||||
|
0.0
|
||||||
|
],
|
||||||
|
"Gripper rotation angle": 1.5707963267948966,
|
||||||
|
"Grip size": 50.0,
|
||||||
|
"OperationType": 0,
|
||||||
|
"Operation Priority": 0,
|
||||||
|
"a.Operation Parameter 1": 0.0,
|
||||||
|
"a.Operation Parameter 2": 0.0,
|
||||||
|
"a.Operation Parameter 3": 0.0
|
||||||
|
}
|
||||||
|
}
|
||||||
BIN
simgrip.FCStd
Normal file
BIN
simgrip.FCStd
Normal file
Binary file not shown.
Loading…
Add table
Add a link
Reference in a new issue