/
pymates.py
309 lines (280 loc) · 14.1 KB
/
pymates.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
#!/usr/bin/python
#pymates
#ipython -wthread -c "import pymates" -i
#pymates.start()
#pymates.demo()
#pymates.mate_parts()
#optionally, pymates.move(pymates.total_parts[0],x,y,z,i,i,i,j,j,j)
#see ~/local/pythonOCC/samples/Level2/DataExchange/import_step_multi.py
#see ~/local/pythonOCC/samples/Level1/TopologyTransformation/mirror.py
import yaml
import re
import os
#os.environ['CSF_GraphicShr'] = r"/usr/lib/libTKOpenGl.so"
import time
import random
import numpy
import wx
import OCC.gp
import OCC.BRepPrimAPI
import OCC.BRepBuilderAPI
import OCC.Display.wxSamplesGui
import OCC.Utils.DataExchange.STEP
import geom
import mate
total_parts = []
# the following aren't our responsibility, actually (pythonOCC?)
#class Circle(yaml.YAMLObject)
#class Cylinder(yaml.YAMLObject)
#class InterfaceGeom(yaml.YAMLObject):
class Part(yaml.YAMLObject):
'''used for part mating. argh I hope OCC doesn't already implement this and I just don't know it.
should a part without an interface be invalid?'''
yaml_tag = '!part'
def __init__(self, description="description", created=time.localtime(), files=[], interfaces={}):
self.description, self.created, self.files, self.interfaces = description, created, files, interfaces
def load_CAD(self):
if len(self.files) == 0: return #no files to load
#FIXME: assuming STEP
for file in self.files:
my_step_importer = OCC.Utils.DataExchange.STEP.STEPImporter(str(file))
my_step_importer.ReadFile()
self.shapes = my_step_importer.GetShapes()
self.compound = my_step_importer.GetCompound()
result = OCC.Display.wxSamplesGui.display.DisplayShape(self.shapes)
if type(result) == type([]): self.ais_shapes = result[0]
else: self.ais_shapes = result
i, j, k, point = self.interfaces[self.interfaces.keys()[0]].i, self.interfaces[self.interfaces.keys()[0]].j, self.interfaces[self.interfaces.keys()[0]].k, self.interfaces[self.interfaces.keys()[0]].point
self.transform = [
[i[0], j[0], k[0], point[0]],
[i[1], j[1], k[1], point[1]],
[i[2], j[2], k[2], point[2]],
[0, 0, 0, 1]
]
return
def __repr__(self):
return "%s(description=%s, created=%s, files=%s, interfaces=%s)" % (self.__class__.__name__, self.description, self.created, self.files, self.interfaces)
def yaml_repr(self):
return "description: %s\ncreated: %s\nfiles: %s\ninterfaces: %s" % (self.description, self.created, self.files, self.interfaces)
def __setstate__(self, attrs):
#print "Part.__setstate__ says attrs = ", attrs
for (k,v) in attrs.items():
#self.__setattr__(each[0],each[1])
if type(v) == Interface:
v.name = k
if hasattr(self, "interfaces"): self.interfaces[k] = v
else:
self.interfaces = {}
self.interfaces[k] = v
self.__setattr__(k,v)
#@classmethod
#def to_yaml(cls, dumper, data):
# return dumper.represent_mapping(cls.yaml_tag, cls.yaml_repr(data))
#@classmethod
#def from_yaml(cls, loader, node):
# print "from_yaml() says that loader = ", loader
# data = loader.construct_mapping(node)
# print "from_yaml() says that data = ", data
# return cls(data)
class Interface(yaml.YAMLObject):
'''"units" should be what is being transmitted through the interface, not about the structure.
a screw's head transmits a force (N), but not a pressure (N/m**2) because the m**2 is actually interface geometry'''
yaml_tag = '!interface'
def __init__(self, name="generic interface name", units=None, geometry=None, point=[0,0,0], i=[0,0,0], j=[0,0,0], k=[0,0,0]):
self.name, self.units, self.geometry, self.point, self.i, self.j, self.k = name, units, geometry, points, numpy.matrix(i), numpy.matrix(j), numpy.matrix(k)
def __repr__(self):
return "Interface(name=%s,units=%s,geometry=%s,point=%s,i=%s,j=%s,k=%s)" % (self.name, self.units, self.geometry, self.point, self.i, self.j, self.k)
def yaml_repr(self):
return "name: %s\nunits: %s\ngeometry: %s\npoint: %s\ni: %s\nj: %s\nk: %s" % (self.name, self.units, self.geometry, self.point, self.i, self.j, self.k)
#@classmethod
#def to_yaml(cls, dumper, data):
# return dumper.represent_scalar(cls.yaml_tag, cls.yaml_repr(data))
#@classmethod
#def from_yaml(cls, loader, node):
# return Interface("node name from from_yaml")
#for cls in [Part, Interface]:
# yaml.add_implicit_resolver(cls.yaml_tag, cls.yaml_pattern)
def compatibility(part1, part2):
'''find all possible combinations of part1 and part2 (for each interface/port) and check each compatibility'''
return []
def compatibility(part1port, part2port):
'''note that an interface/port object refers to what it is on. so you don't have to pass the parts.'''
return []
def load(foo):
return yaml.load(foo)
def dump(foo):
return yaml.dump(foo, default_flow_style=False)
def demo(event=None):
print "loading the file .. it looks like this:"
blockhole = load(open("models/blockhole.yaml"))["blockhole"]
peg = load(open("models/peg.yaml"))["peg"]
print "blockhole is = ", dump(blockhole)
print "peg is = ", dump(peg)
#load the CAD?
#load_cad_file(filename=blockhole.files[0])
blockhole.load_CAD()
peg.load_CAD()
total_parts.append(blockhole)
total_parts.append(peg)
def demo2(event=None, part=Part()):
'''reposition the part to be at one of the interfaces of the part. this replaces move_parts().'''
if not part.interfaces or len(part.interfaces) == 0:
if len(total_parts) == 0: return #can't do anything about that, can we
part = total_parts[0]
if len(part.interfaces) == 0: return #ok I give up
#select the first interface
interface = part.interfaces[part.interfaces.keys()[0]][0]
point = interface[part.interfaces.keys()[0]][0].point
i = interface.i
j = interface.j
k = interface.k
o_point = OCC.gp.gp_Pnt(point[0], point[1], point[2])
o_n_vec = OCC.gp.gp_Dir(i[0], i[1], i[2])
o_vx_vec = OCC.gp.gp_Dir(j[0], j[1], j[2])
ax3 = OCC.gp.gp_Ax3(o_point, o_n_vec, o_vx_vec)
the_transform = OCC.gp.gp_Trsf()
#myax = OCC.gp.gp_Ax3( blah )
#myax.Transform(the_transform)
the_transform.SetTransformation(ax3)
the_toploc = OCC.TopLoc.TopLoc_Location(the_transform)
#import OCC.AIS
#OCC.AIS.Handle_AIS_InteractiveObject()
OCC.Display.wxSamplesGui.display.Context.SetLocation(part.ais_shapes, the_toploc)
OCC.Display.wxSamplesGui.display.Context.UpdateCurrentViewer()
return
def load_cad_file(event=None, filename=""):
'''deprecated'''
pass
if not filename or filename == "":
#popup menu selector for finding a filename
filename = wx.FileSelector()
#FIXME: this assumes that the path is relative to the curdir- i.e. in skdb/pymates/models/ or at least skdb/pymates/
#figure out relative path for STEPImporter
fullpath = os.path.realpath(os.path.curdir)
filename = filename.replace(fullpath + "/","")
#load the STEP file
my_step_importer = OCC.Utils.DataExchange.STEP.STEPImporter(str(filename))
my_step_importer.ReadFile()
the_shapes = my_step_importer.GetShapes()
the_compound = my_step_importer.GetCompound()
#don't forget to get the return value and append it to total_shapes
#FIXME: don't be so lame re: use of globals.
ais_shapes = OCC.Display.wxSamplesGui.display.DisplayShape(the_shapes)
total_parts.append(ais_shapes[0]) #sorry
def mate_parts(event=None):
#mate all of the parts in the workspace
if len(total_parts) < 1: return #meh
print "numpy.matrix(total_parts[0].transform) = \n", numpy.matrix(total_parts[0].transform)
print "numpy.matrix(total_parts[1].transform) = \n", numpy.matrix(total_parts[1].transform)
print "numpy.matrix(total_parts[0].transform).I = \n", numpy.matrix(total_parts[0].transform).I
T = numpy.matrix(total_parts[1].transform) * numpy.matrix(total_parts[0].transform).I
print "the transform T is = \n", T
#T = (A * B.I)
#A = B * (A * B.I)
#B = A * T
result = numpy.matrix(total_parts[0].transform) * T
print "result = \n", result
total_parts[1].transform = result #this doesn't do anything
print "old point = \n", total_parts[1].interfaces[total_parts[1].interfaces.keys()[0]].point
#numpy.matrix([[1,2],[3,4]]).getA()[0] = [1,2].
interface = total_parts[1].interfaces[total_parts[1].interfaces.keys()[0]]
interface.point[0] = (total_parts[1].transform.tolist())[0][3]
interface.point[1] = total_parts[1].transform.tolist()[1][3]
interface.point[2] = total_parts[1].transform.tolist()[2][3]
print "new point = \n", total_parts[1].interfaces[total_parts[1].interfaces.keys()[0]].point
point = interface.point
lresult = result.tolist()
interface.i = [lresult[0][0], lresult[1][0], lresult[2][0]]
interface.j = [lresult[0][1], lresult[1][1], lresult[2][1]]
interface.k = [lresult[0][2], lresult[1][2], lresult[2][2]]
i, j, k = interface.i, interface.k, interface.j
o_point = OCC.gp.gp_Pnt(point[0], point[1]-10, point[2]-5)
o_n_vec = OCC.gp.gp_Dir(0,0,1)#(i[0], i[1], i[2])
print "the j vector is going to be: ", j
o_vx_vec = OCC.gp.gp_Dir(0,1,0)#j[0], j[1], j[2])
# pymates.move(pymates.total_parts[1], 5,5,-10, 0,0,1, -1,0,1)
ax3 = OCC.gp.gp_Ax3(o_point, o_n_vec, o_vx_vec)
transform2 = OCC.gp.gp_Trsf()
transform2.SetTransformation(ax3)
toploc = OCC.TopLoc.TopLoc_Location(transform2)
##Build original shape
#original_shape = BRepPrimAPI_MakeWedge(60.,100.,80.,20.).Shape()
##Define transformation
#transformation = OCC.gp.gp_Trsf()
#pnt_center_of_the_transformation = OCC.gp.gp_Pnt(110.,60.,60.)
#V = OCC.BRepBuilderAPI.BRepBuilderAPI_MakeVertex(pnt_center_of_the_transformation)
#transformation.SetMirror(pnt_center_of_the_transformation)
##see also transformation.SetTransformation(fromCoordinateSystem1, toCoordinateSystem2)
##Apply the transformation
##like by P2 = P1.Transformed(transformation) - but not all Shape() objects have a Transformed(?) method
#my_brep_transformation = OCC.BRepBuilderAPI.BRepBuilderAPI_Transform(original_shape, transformation)
#transformed_shape = my_brep_transformation.Shape()
##now display original_shape and transformed_shape()
#take the cross product o_n_vec and o_vx_vec - check if they are consistent with themselves (if they are orthogonal)
#which gives you what to put in the center column
#now find out the 2nd column in the 4x4 (the missing vy_vec in the 4x4)
# a cross b = magnitude(a)*magnitude(b) * sin theta * (n - the unit vector perpendicular to the plane containing a & b)
# you want theta to be 90
# sqrt ( Vyx^2 + Vyy^2 + Vyz^2) = absolute value of Vy
# absolute value of Vy = absolute value of Vn * absolute value of Vx * sin(theta)
# theta = arcsin( sqrt(Vyx^2 + vyy^2 + Vyz^2) / (absolute value of Vn * absolute value of Vx) )
# arcsin(1) = 90 degrees
# figure out that cross product to figure out the central term
# check that the magnitudes of Vn and Vx equal the magnitude of Vy .. |Vn|*|Vx| must = |Vy|
#change it to a cone (instead of a peg)
#now to get the z vectors correct.
#rotate the interface first by its x-axis (180 degrees) (swap the signs of all values in 2nd&3rd columns)
OCC.Display.wxSamplesGui.display.DisplayShape(shape_here)
return
def move(my_part, x, y, z, i1, i2, i3, j1, j2, j3):
o_point = OCC.gp.gp_Pnt(x,y,z)
o_n_vec = OCC.gp.gp_Dir(i1,i2,i3)
o_vx_vec = OCC.gp.gp_Dir(j1,j2,j3)
ax3 = OCC.gp.gp_Ax3(o_point, o_n_vec, o_vx_vec)
transform = OCC.gp.gp_Trsf()
transform.SetTransformation(ax3)
toploc = OCC.TopLoc.TopLoc_Location(transform)
OCC.Display.wxSamplesGui.display.Context.SetLocation(my_part.ais_shapes, toploc)
OCC.Display.wxSamplesGui.display.Context.UpdateCurrentViewer()
return
def move_parts(event=None):
if len(total_parts) == 0: return
if len(total_parts) == 1: working_part = total_parts[0]
else: working_part = total_parts[random.randrange(0,len(total_parts))]
#gp_Dir, gce_MakeDir, Geom_Direction: http://adl.serveftp.org/lab/opencascade/doc/ReferenceDocumentation/FoundationClasses/html/classgp__Dir.html
#gp_Ax3: http://adl.serveftp.org/lab/opencascade/doc/ReferenceDocumentation/FoundationClasses/html/classgp__Ax3.html
#gp_Ax3 (const gp_Pnt &P, const gp_Dir &N, const gp_Dir &Vx)
#gp_Ax3 (const gp_Pnt &P, const gp_Dir &V)
#see pythonOCC/samples/Level1/Animation/animation.py
point = OCC.gp.gp_Pnt(0,0,0)
n_vec = OCC.gp.gp_Dir(0,0,1)
tempvar = [1,1,1] #[-1,0,0] #[0,-1,0]
#TODO: check whether or not tempvar is valid
vx_vec = OCC.gp.gp_Dir(tempvar[0],tempvar[1],tempvar[1])
ax3 = OCC.gp.gp_Ax3(point, n_vec, vx_vec)
the_transform = OCC.gp.gp_Trsf()
the_transform.SetTransformation(ax3)
the_toploc = OCC.TopLoc.TopLoc_Location(the_transform)
OCC.Display.wxSamplesGui.display.Context.SetLocation(working_shape, the_toploc)
OCC.Display.wxSamplesGui.display.Context.UpdateCurrentViewer()
return
def add_part_mate(part_1_interface, part_2_interface):
#mate interface 1 to interface 2 (move part 1)
#return the ID of the shape transformation added to the part object
return
def show_part_mate(part_mate_object):
#look at the part_mate_object and DisplayShape() the shape with the right coordinate system
return
def start():
OCC.Display.wxSamplesGui.display.Create()
def exit(event=None):
import sys; sys.exit()
if __name__ == '__main__':
OCC.Display.wxSamplesGui.add_menu("do stuff")
OCC.Display.wxSamplesGui.add_function_to_menu('do stuff', demo)
OCC.Display.wxSamplesGui.add_function_to_menu('do stuff', demo2)
OCC.Display.wxSamplesGui.add_function_to_menu('do stuff', load_cad_file)
OCC.Display.wxSamplesGui.add_function_to_menu('do stuff', mate_parts)
OCC.Display.wxSamplesGui.add_function_to_menu('do stuff', move_parts)
OCC.Display.wxSamplesGui.add_function_to_menu('do stuff', exit)
OCC.Display.wxSamplesGui.start_display()