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")