def test_gp_Quaternion(self):
'''
Test Interpolate method of qp_QuaternionSLerp.
This method takes a by ref parameter q.
'''
vX = gp_Vec(12, 0, 0)
vY = gp_Vec(0, 12, 0)
q = gp_Quaternion()
q1 = gp_Quaternion(vX, vX)
q2 = gp_Quaternion(vX, vY)
interp = gp_QuaternionSLerp(q1, q2)
interp.Init(q1, q2)
for i in range(10):
i__ = i / 10.
interp.Interpolate(i__, q)
if i == 0:
self.assertEqual(q.X(), 0.)
self.assertEqual(q.Y(), 0.)
self.assertEqual(q.Z(), 0.)
self.assertEqual(q.W(), 1.)
else:
self.assertEqual(q.X(), 0.)
self.assertEqual(q.Y(), 0.)
self.assertGreater(q.Z(), 0.)
self.assertLess(q.W(), 1.)
def test_gp_Quaternion(self):
'''
Test Interpolate method of qp_QuaternionSLerp.
This method takes a by ref parameter q.
'''
vX = gp_Vec(12, 0, 0)
vY = gp_Vec(0, 12, 0)
v45 = (gp_Vec(1, 1, 1).Normalized() * 12)
q = gp_Quaternion()
q1 = gp_Quaternion(vX, vX)
q2 = gp_Quaternion(vX, vY)
interp = gp_QuaternionSLerp(q1, q2)
interp.Init(q1, q2)
for i in range(10):
i__ = i / 10.
interp.Interpolate(i__, q)
if i == 0:
self.assertEqual(q.X(), 0.)
self.assertEqual(q.Y(), 0.)
self.assertEqual(q.Z(), 0.)
self.assertEqual(q.W(), 1.)
else:
self.assertEqual(q.X(), 0.)
self.assertEqual(q.Y(), 0.)
assert q.Z() > 0.
assert q.W() < 1.
def htransform2(tpnt, tvx, tvy, tvz, pnt, vx, vy, vz):
tvx = tvy.Crossed(tvz)
tvy = tvz.Crossed(tvx)
tvx.Normalize()
tvy.Normalize()
tvz.Normalize()
tpnt = gp_Pnt((gp_Vec(tpnt.XYZ()) + tvz * 0.1).XYZ())
vx = vy.Crossed(vz)
vy = vz.Crossed(vx)
vx.Normalize()
vy.Normalize()
vz.Normalize()
Tt = gp_GTrsf(
gp_Mat(tvx.X(), tvy.X(), tvz.X(), tvx.Y(), tvy.Y(), tvz.Y(), tvx.Z(),
tvy.Z(), tvz.Z()), gp_XYZ(tpnt.X(), tpnt.Y(), tpnt.Z()))
Tt.Invert()
rott = gp_Mat(tvx.X(), tvy.X(), tvz.X(), tvx.Y(), tvy.Y(), tvz.Y(),
tvx.Z(), tvy.Z(), tvz.Z())
rott.Transpose()
quatt = gp_Quaternion(rott)
dispt = gp_Vec(Tt.TranslationPart().X(),
Tt.TranslationPart().Y(),
Tt.TranslationPart().Z())
trsft = gp_Trsf()
trsft.SetTransformation(quatt, dispt)
rotp = gp_Mat(vx.X(), vy.X(), vz.X(), vx.Y(), vy.Y(), vz.Y(), vx.Z(),
vy.Z(), vz.Z())
quatp = gp_Quaternion(rotp)
dispp = gp_Vec(gp_Pnt(0, 0, 0), pnt)
trsfp = gp_Trsf()
trsfp.SetTransformation(quatp, dispp)
trsfo = trsfp.Multiplied(trsft)
loco = TopLoc_Location(trsfo)
return loco
def filling(self, name_body, coord_centres, coord_centres_2):
'''
:param name_body:
:param coord_centres: 0 - угол поворота x (градусы)
1 - угол поворота y (градусы)
2 - угол поворота z (градусы)
3 - смещенмие по x
4 - смещенмие по y
5 - смещенмие по z
:return:
'''
# print(name_body, coord_centres)
# shape = self.modules[name_body]
# print(body)
cp = BRepBuilderAPI_Copy(self.reserv_models[name_body])
cp.Perform(self.reserv_models[name_body])
shape = cp.Shape()
# print(shape)
r = R.from_euler(
'xyz', [coord_centres[0], coord_centres[1], coord_centres[2]],
degrees=True)
Rot = r.as_dcm()
trsf = gp_Trsf()
Mat = gp_Mat(Rot[0][0], Rot[0][1], Rot[0][2], Rot[1][0], Rot[1][1],
Rot[1][2], Rot[2][0], Rot[2][1], Rot[2][2])
trsf.SetRotation(gp_Quaternion(Mat))
shape.Move(TopLoc_Location(trsf))
r = R.from_euler(
'xyz',
[coord_centres_2[0], coord_centres_2[1], coord_centres_2[2]],
degrees=True)
Rot = r.as_dcm()
trsf = gp_Trsf()
Mat = gp_Mat(Rot[0][0], Rot[0][1], Rot[0][2], Rot[1][0], Rot[1][1],
Rot[1][2], Rot[2][0], Rot[2][1], Rot[2][2])
trsf.SetRotation(gp_Quaternion(Mat))
shape.Move(TopLoc_Location(trsf))
trsf = gp_Trsf()
trsf.SetTranslation(
gp_Vec(coord_centres[3], coord_centres[4], coord_centres[5]))
shape.Move(TopLoc_Location(trsf))
self.modules[name_body] = shape
def rotate(event=None):
display.EraseAll()
origin = gp_Vec(0, 0, 0)
origin_pt = as_pnt(origin)
vX = gp_Vec(12, 0, 0)
vY = gp_Vec(0, 12, 0)
v45 = (gp_Vec(1, 1, 1).Normalized() * 12)
q1 = gp_Quaternion(vX, vY)
p1 = as_pnt(origin + vX)
p2 = as_pnt(origin + vY)
p3 = as_pnt(origin + (q1 * vY))
p4 = as_pnt(origin + (q1 * v45))
# RED
e1 = make_edge(origin_pt, p1)
e2 = make_edge(origin_pt, p2)
e3 = make_edge(origin_pt, as_pnt(v45))
# GREEN -> transformed
e4 = make_edge(origin_pt, p3)
e5 = make_edge(origin_pt, p4)
display.DisplayShape([e1, e2, e3])
display.DisplayColoredShape([e4, e5], 'GREEN')
display.DisplayMessage(p1, 'e1')
display.DisplayMessage(p2, 'e2')
display.DisplayMessage(as_pnt(v45), 'e3')
display.DisplayMessage(p3, 'q1*vY')
display.DisplayMessage(p4, 'q1*v45')
display.DisplayVector((q1 * vY).Normalized(), as_pnt(origin + q1 * vY / 2.))
display.DisplayVector((q1 * v45).Normalized(), as_pnt(origin + q1 * v45 / 2.))
display.FitAll()
def x3d_display(shape,
vertex_shader=None,
fragment_shader=None,
export_edges=True,
color=(1, 1, 0),
specular_color=(1, 1, 1),
shininess=0.4,
transparency=0.4,
line_color=(0, 0, 0),
line_width=2.,
mesh_quality=.3):
# Export to XML <Scene> tag
exporter = X3DExporter(shape, vertex_shader, fragment_shader, export_edges,
color, specular_color, shininess, transparency,
line_color, line_width, mesh_quality)
exporter.compute()
x3d_str = exporter.to_x3dfile_string(shape_id=0)
xml_et = ElementTree.fromstring(x3d_str)
scene_tag = xml_et.find('./Scene')
# Viewport Parameters
bb = BoundBox._fromTopoDS(shape)
d = max(bb.xlen, bb.ylen, bb.zlen)
c = bb.center
vec = gp_Vec(0, 0, d / 1.5 / tan(FOV / 2))
quat = gp_Quaternion(*ROT)
vec = quat * (vec) + c.wrapped
# return boilerplate + Scene
return add_x3d_boilerplate(ElementTree.tostring(scene_tag).decode('utf-8'),
d=(vec.X(), vec.Y(), vec.Z()),
center=(c.x, c.y, c.z))
def update(links, T, tool=None):
for i in range(0, len(links)):
display.Context.SetLocation(links[i], TopLoc_Location(T[i]))
if tool != None and i == len(links) - 1:
M = gp_Mat(0, 0, 1, 0, -1, 0, 1, 0, 0)
P = gp_Pnt(100, 0, 0)
T_ee = gp_Trsf()
T_ee.SetTransformation(gp_Quaternion(M),\
gp_Vec(gp_Pnt(),P))
display.Context.SetLocation(
tool, TopLoc_Location(T[i].Multiplied(T_ee)))
display.Context.UpdateCurrentViewer()
def read_stp_solid_withTf(modelDir, stpFilename, vecXYZlist, quaternionXYZWlist, unitIsMM=False):
solid = read_stp_as_solid(os.path.join(modelDir, stpFilename + ".stp"))
trsf = gp_Trsf()
q = gp_Quaternion()
if not unitIsMM:
vecXYZlist = [i * 1000.0 for i in vecXYZlist]
q.Set(quaternionXYZWlist[0], quaternionXYZWlist[1], quaternionXYZWlist[2], quaternionXYZWlist[3])
trsf.SetTransformation(q, gp_Vec(vecXYZlist[0], vecXYZlist[1], vecXYZlist[2]))
toploc = TopLoc_Location(trsf)
solid.Move(toploc)
return solid
def interpolate(event=None):
display.EraseAll()
origin = gp_Vec()
vX = gp_Vec(12, 0, 0)
vY = gp_Vec(0, 12, 0)
v45 = (gp_Vec(1, 1, 1).Normalized() * 12)
q = gp_Quaternion()
interp = gp_QuaternionSLerp(gp_Quaternion(vX, vX), gp_Quaternion(vX, vY))
for i in frange(0, 1.0, 0.01):
interp.Interpolate(i, q)
# displace the white edges a little from the origin so not to obstruct the other edges
v = gp_Vec(0, -24*i, 0)
q_v_ = q * v45
p = gp_Pnt((q_v_ + v).XYZ())
v__as_pnt = gp_Pnt((origin + v).XYZ())
e = make_edge(v__as_pnt, p)
display.DisplayColoredShape(e, 'WHITE')
msg = 'v45->q1*v45 @{0}'.format(i / 10.)
display.DisplayMessage(p, msg)
display.FitAll()
def FK(robot_model, joint_var):
mod_joint_var = numpy.concatenate([[0], joint_var])
A = robot_model.forward_kinematics(mod_joint_var, True)
# A = A[:-1]
T = []
for a in A:
M = gp_Mat(a[0][0],a[0][1],a[0][2],\
a[1][0],a[1][1],a[1][2],\
a[2][0],a[2][1],a[2][2])
P = gp_Pnt(a[0][3] * 1000, a[1][3] * 1000, a[2][3] * 1000)
trsf = gp_Trsf()
trsf.SetTransformation(gp_Quaternion(M),\
gp_Vec(gp_Pnt(),P))
T.append(trsf)
return T
def reverse_tool_transform(points):
M = gp_Mat(-0.955248, -0.295806, 0.0000000000,\
-0.295806, 0.955248, 0.0000000000,\
0.0000000000, 0.0000000000, -1.00000)
P = gp_Pnt(-20.5609, -11.9004, 251.300)
tool_trsf = gp_Trsf()
tool_trsf.SetTransformation(gp_Quaternion(M),\
gp_Vec(gp_Pnt(),P))
tool_trsf.Invert()
M = gp_Mat(0, 0, 1, 0, -1, 0, 1, 0, 0)
P = gp_Pnt(100, 0, 0)
ee_trsf = gp_Trsf()
ee_trsf.SetTransformation(gp_Quaternion(M),\
gp_Vec(gp_Pnt(),P))
ee_trsf.Invert()
total_trsf = tool_trsf.Multiplied(ee_trsf)
new_points = []
for point in points:
new_point = []
M = gp_Mat(point[1].X(),point[2].X(),point[3].X(),\
point[1].Y(),point[2].Y(),point[3].Y(),
point[1].Z(),point[2].Z(),point[3].Z())
P = point[0]
point_trsf = gp_Trsf()
point_trsf.SetTransformation(gp_Quaternion(M),\
gp_Vec(gp_Pnt(),P))
new_point_trsf = point_trsf.Multiplied(total_trsf)
M = new_point_trsf.VectorialPart()
new_point.append(gp_Pnt(new_point_trsf.TranslationPart()))
new_point.append(gp_Vec(M.Column(1)).Normalized())
new_point.append(gp_Vec(M.Column(2)).Normalized())
new_point.append(gp_Vec(M.Column(3)).Normalized())
new_points.append(new_point)
return new_points
def htransform(shape, pnt, vx, vy, vz):
vx = vy.Crossed(vz)
vy = vz.Crossed(vx)
vx.Normalize()
vy.Normalize()
vz.Normalize()
tshape = copy.deepcopy(shape)
rot = gp_Mat(vx.X(), vy.X(), vz.X(), vx.Y(), vy.Y(), vz.Y(), vx.Z(),
vy.Z(), vz.Z())
quat = gp_Quaternion(rot)
disp = gp_Vec(gp_Pnt(0, 0, 0), pnt)
trsf = gp_Trsf()
trsf.SetTransformation(quat, disp)
aniloc = TopLoc_Location(trsf)
loc = aniloc * tshape.Location()
tshape.Location(loc)
return aniloc, tshape
# c.Green(),
# c.Blue(),
# Quantity_TOC_RGB))
shp = read_step_file(os.path.join('part_of_sattelate', 'pribore', 'DAV_WS16.STEP'))
#BRepOffsetAPI_MakeOffsetShape(shp, 10, 0.1)
#kode to rotation
trsf = gp_Trsf()
# vX = gp_Vec(12, 0, 0)
# vY = gp_Vec(0, 12, 0)
Mat = gp_Mat(0.5, (0.75**0.5), 0,
-(0.75**2), 0.5, 0,
0, 0, 1)
trsf.SetRotation(gp_Quaternion(Mat))
shp.Move(TopLoc_Location(trsf))
def measure(shape):
bbox = Bnd_Box()
#bbox.SetGap(tol)
#bbox.SetShape(shape)
brepbndlib_Add(shape, bbox)
xmin, ymin, zmin, xmax, ymax, zmax = bbox.Get()
my_box = BRepPrimAPI_MakeBox(gp_Pnt(xmin, ymin, zmin), gp_Pnt(xmax, ymax, zmax)).Shape()
#display.SetBackgroundImage('nevers-pont-de-loire.jpg', stretch=True)
display.DisplayShape(my_box, color='GREEN', transparency=0.9)
print(zmin, zmax, 'zzzzzzzzzzzzz')
if __name__ == '__main__':
argvs = sys.argv
parser = argparse.ArgumentParser()
parser.add_argument("--dir", dest="dir", default="./")
parser.add_argument("--pxyz",
dest="pxyz",
default=[0.0, 0.0, 0.0],
type=float,
nargs=3)
opt = parser.parse_args()
print(opt, argvs)
obj = dispocc(touch=True)
axs = gp_Ax3(gp_Pnt(100, -100, 200), gp_Dir(0, 0.5, 1.0),
gp_Dir(0.5, 1.0, 0))
qp = gp_Quaternion()
qp.SetRotation(gp_Vec(0, 0, 100), gp_Vec(50, 0, 0))
print(qp)
obj.show_axs_pln(axs, scale=50, name="axs0")
trf = gp_Trsf()
trf.SetTransformation(qp, gp_Vec(0, 0, 1))
ax1 = axs.Transformed(trf)
obj.show_axs_pln(ax1, scale=50, name="axs1")
ax2 = obj.prop_axs(ax1, scale=50)
obj.show_axs_pln(ax2, scale=50, name="axs2")
ax3 = obj.rot_axis(ax2, xyz="z", deg=45)
obj.show_axs_pln(ax3, scale=50)
pln1 = obj.show_plane(ax3, scale=25, trs=0.9, color="RED")
