def get_std_convexhull(pcd, origin="center", color=(1, 1, 1), transparency=1, toggledebug=False, toggleransac=True): """ create CollisionModel by pcd, standardized rotation :param pcd: :param origin: "center" or "tip" :param color: :param transparency: :param toggledebug: :param toggleransac: :return: CollisionModel, position of origin """ rot_angle = get_rot_frompcd(pcd, toggledebug=toggledebug, toggleransac=toggleransac) center = get_pcd_center(pcd) origin_pos = np.array(center) pcd = pcd - np.array([center]).repeat(len(pcd), axis=0) pcd = trans_pcd( pcd, rm.homobuild((0, 0, 0), rm.rodrigues((0, 0, 1), -rot_angle))) convexhull = trimesh.Trimesh(vertices=pcd) convexhull = convexhull.convex_hull obj = cm.CollisionModel(initor=convexhull) obj_w, obj_h = get_pcd_w_h(pcd) if origin == "tip": tip = get_pcd_tip(pcd, axis=0) origin_pos = center + \ trans_pcd(np.array([tip]), rm.homobuild((0, 0, 0), rm.rodrigues((0, 0, 1), rot_angle)))[0] pcd = pcd - np.array([tip]).repeat(len(pcd), axis=0) convexhull = trimesh.Trimesh(vertices=pcd) convexhull = convexhull.convex_hull obj = cm.CollisionModel(initor=convexhull) if toggledebug: obj.set_rgba(color[0], color[1], color[2], transparency) obj.reparentTo(base.render) obj.showlocalframe() print("Rotation angle:", rot_angle) print("Origin:", center) base.pggen.plotSphere(base.render, center, radius=2, rgba=(1, 0, 0, 1)) return obj, obj_w, obj_h, origin_pos, rot_angle
def gen_arrow(spos=np.array([0, 0, 0]), epos=np.array([0.1, 0, 0]), thickness=0.005, sections=8, sticktype="rect"): """ :param spos: 1x3 nparray :param epos: 1x3 nparray :param thickness: 0.005 m by default :param sections: # of discretized sectors used to approximate a cylinder :param sticktype: The shape at the end of the arrow stick, round or rect :param radius: :return: author: weiwei date: 20191228osaka """ direction = rm.unit_vector(epos - spos) stick = gen_stick(spos=spos, epos=epos - direction * thickness * 4, thickness=thickness, type=sticktype, sections=sections) cap = gen_cone(spos=epos - direction * thickness * 4, epos=epos, radius=thickness, sections=sections) vertices = np.vstack((stick.vertices, cap.vertices)) capfaces = cap.faces + len(stick.vertices) faces = np.vstack((stick.faces, capfaces)) return trm.Trimesh(vertices=vertices, faces=faces)
def drawTheCovexHull(constraints, pos=[150, 150, 150]): print("The constraints are:",constraints) print("The len is",len(constraints)) base.pggen.plotAxis(base.render, spos=np.array(pos), length=80, thickness=3) convexVetices = [np.array(pos)] convexVeticesNormal = [] for i in range(0, len(constraints)): base.pggen.plotArrow(base.render, spos=pos, epos=pos + constraints[i] * 40, thickness=4, rgba=[0,0,0, 0.7]) convexVetices.append((pos + constraints[i] * 40)) convexVeticesNormal.append(constraints[i]) trimeshNew = trimesh.Trimesh(vertices=np.array(convexVetices)) try: convexHull = trimeshNew.convex_hull convexHullpd = pg.trimeshtonp(convexHull) convexHullpd.setColor(0,0,0,0.2 ) convexHullpd.setTransparency(TransparencyAttrib.MAlpha) convexHullpd.reparentTo(base.render) except: print("Cannot generate the convex hull") bestDirection = np.array([0, 0, 0]) for z in constraints: bestDirection = bestDirection + z bestDirection = rm.unit_vector(bestDirection) base.pggen.plotArrow(base.render, spos=pos, epos=pos + bestDirection * 40, thickness=4, rgba=[147.0 / 255, 112.0 / 255, 219.0 / 255, 0.7]) print("The best value is ", bestDirection)
def gen_dasharrow(spos=np.array([0, 0, 0]), epos=np.array([0.1, 0, 0]), thickness=0.005, lsolid=None, lspace=None, sections=8, sticktype="rect"): """ :param spos: 1x3 nparray :param epos: 1x3 nparray :param thickness: 0.005 m by default :param lsolid: length of the solid section, 1*thickness if None :param lspace: length of the empty section, 1.5*thickness if None :return: author: weiwei date: 20191228osaka """ length, direction = rm.unit_vector(epos - spos, toggle_length=True) cap = gen_cone(spos=epos - direction * thickness * 4, epos=epos, radius=thickness, sections=sections) dash_stick = gen_dashstick(spos=spos, epos=epos - direction * thickness * 4, thickness=thickness, lsolid=lsolid, lspace=lspace, sections=sections, sticktype=sticktype) tmp_stick_faces = dash_stick.faces + len(cap.vertices) vertices = np.vstack((cap.vertices, dash_stick.vertices)) faces = np.vstack((cap.faces, tmp_stick_faces)) return trm.Trimesh(vertices=vertices, faces=faces)
def drawanySingleSurface(a, vertices, color): ''' draw a surface using a calculated fourth point to creat a hull :param base: :param vertices: :param faces: :param color: :return: ''' # print("faces in plotsurface",faces) surface_vertices = vertices # surface = humath.centerPoftrangle(surface_vertices[0][:3], surface_vertices[1][:3], surface_vertices[2][:3]) surface = trimesh.Trimesh(surface_vertices) surface = surface.convex_hull surface = gm.GeometricModel(surface) if color == "red": rgba = (1, 0, 0, 1) elif color == "green": rgba = (61 / 255, 145 / 255, 64 / 255, 1) elif color == "orange": rgba = (255 / 255, 100 / 255, 24 / 255, 1) elif color == "white": rgba = (1, 1, 1, 1) elif color == "blue": # rgba = (3/255,168/255,158/255,1) rgba = (0 / 255, 0 / 255, 255 / 255, 1) elif color == "yellow": rgba = (1, 1, 0, 1) else: rgba = (0, 0, 0, 1) surface.set_rgba(rgba) surface.objpdnp.reparentTo(a)
def gen_dumbbell(spos=np.array([0, 0, 0]), epos=np.array([0.1, 0, 0]), thickness=0.005, sections=8, subdivisions=1): """ NOTE: return stick+spos_ball+epos_ball also work, but it is a bit slower :param spos: 1x3 nparray :param epos: 1x3 nparray :param thickness: 0.005 m by default :param sections: :param subdivisions: levels of icosphere discretization :return: author: weiwei date: 20191228osaka """ stick = gen_rectstick(spos=spos, epos=epos, thickness=thickness, sections=sections) spos_ball = gen_sphere(pos=spos, radius=thickness, subdivisions=subdivisions) epos_ball = gen_sphere(pos=epos, radius=thickness, subdivisions=subdivisions) vertices = np.vstack( (stick.vertices, spos_ball.vertices, epos_ball.vertices)) sposballfaces = spos_ball.faces + len(stick.vertices) endballfaces = epos_ball.faces + len(spos_ball.vertices) + len( stick.vertices) faces = np.vstack((stick.faces, sposballfaces, endballfaces)) return trm.Trimesh(vertices=vertices, faces=faces)
def scoreConvexHullOfSpaceOfContactNormals(constraints, show = False): uniqconstraints = removeDuplicate(constraints) vertices = np.array(uniqconstraints + [np.array([0, 0, 0])]) try: hull = ConvexHull(vertices, qhull_options="QJ") equations = hull.equations # print("The surface equation is",equations) centerofconvexhull = np.sum(vertices[hull.vertices],axis=0)/len(hull.vertices) centerofconvexhull = np.append(centerofconvexhull,1) distancetoeachplane = [] for eq in equations: distancetoeachplane.append(abs(np.dot(eq,centerofconvexhull))) # print("The distance array is ", distancetoeachplane) # print("The total distance is ", sum(distancetoeachplane)) if show: convexhull = trimesh.Trimesh(vertices=vertices) convexHull = convexhull.convex_hull convexHullpd = pg.trimeshtonp(convexHull) convexHullpd.setColor(0,0,0,0.2 ) convexHullpd.setTransparency(TransparencyAttrib.MAlpha) convexHullpd.setScale(100) convexHullpd.reparentTo(base.render) return sum(distancetoeachplane) except Exception as e: # print(uniqconstraints) if len(uniqconstraints) >1: return 0 else: return 4
def drawTheArea(polygon1,polygon2, Z, invMat, showthecontact = True): convexHull,intersection = drawIntersectionArea(polygon1=polygon1, polygon2=polygon2,Z=Z,invMat=invMat) if showthecontact: intersectionarea = trimesh.Trimesh(vertices=convexHull) intersectionarea_ch = intersectionarea.convex_hull intersectionarea_np = base.pg.trimeshtonp(intersectionarea_ch) intersectionarea_np.setColor(0,1,0,1) intersectionarea_np.reparentTo(base.render) return convexHull
def gen_dashtorus(axis=np.array([1, 0, 0]), portion=.5, center=np.array([0, 0, 0]), radius=0.1, thickness=0.005, lsolid=None, lspace=None, sections=8, discretization=24): """ :param axis: the circ arrow will rotate around this axis 1x3 nparray :param portion: 0.0~1.0 :param center: the center position of the circ 1x3 nparray :param radius: :param thickness: :param lsolid: length of solid :param lspace: length of space :param sections: # of discretized sectors used to approximate a cylindrical stick :param discretization: number sticks used for approximating a torus :return: author: weiwei date: 20200602 """ assert (0 <= portion <= 1) solidweight = 1.6 spaceweight = 1.07 if not lsolid: lsolid = thickness * solidweight if not lspace: lspace = thickness * spaceweight unit_axis = rm.unit_vector(axis) starting_vector = rm.orthogonal_vector(unit_axis) min_discretization_value = math.ceil(2 * math.pi / (lsolid + lspace)) if discretization < min_discretization_value: discretization = min_discretization_value nsections = math.floor(portion * 2 * math.pi * radius / (lsolid + lspace)) vertices = np.empty((0, 3)) faces = np.empty((0, 3)) for i in range(0, nsections): # TODO wrap up end torus_sec = gen_torus( axis=axis, starting_vector=rm.rotmat_from_axangle( axis, 2 * math.pi * portion / nsections * i).dot(starting_vector), portion=portion / nsections * lsolid / (lsolid + lspace), center=center, radius=radius, thickness=thickness, sections=sections, discretization=discretization) torus_sec_faces = torus_sec.faces + len(vertices) vertices = np.vstack((vertices, torus_sec.vertices)) faces = np.vstack((faces, torus_sec_faces)) return trm.Trimesh(vertices=vertices, faces=faces)
def o3dmesh_to_trimesh(o3dmesh): """ :param o3dmesh: :return: author: weiwei date: 20191210 """ vertices = np.asarray(o3dmesh.vertices) faces = np.asarray(o3dmesh.triangles) face_normals = np.asarray(o3dmesh.triangle_normals) cvterd_trimesh = trimesh.Trimesh(vertices=vertices, faces=faces, face_normals=face_normals) return cvterd_trimesh
def gen_dashstick(spos=np.array([0, 0, 0]), epos=np.array([0.1, 0, 0]), thickness=0.005, lsolid=None, lspace=None, sections=8, sticktype="rect"): """ :param spos: 1x3 nparray :param epos: 1x3 nparray :param thickness: 0.005 m by default :param lsolid: length of the solid section, 1*thickness if None :param lspace: length of the empty section, 1.5*thickness if None :return: author: weiwei date: 20191228osaka """ solidweight = 1.6 spaceweight = 1.07 if not lsolid: lsolid = thickness * solidweight if not lspace: lspace = thickness * spaceweight length, direction = rm.unit_vector(epos - spos, toggle_length=True) nstick = math.floor(length / (lsolid + lspace)) vertices = np.empty((0, 3)) faces = np.empty((0, 3)) for i in range(0, nstick): tmp_spos = spos + (lsolid * direction + lspace * direction) * i tmp_stick = gen_stick(spos=tmp_spos, epos=tmp_spos + lsolid * direction, thickness=thickness, type=sticktype, sections=sections) tmp_stick_faces = tmp_stick.faces + len(vertices) vertices = np.vstack((vertices, tmp_stick.vertices)) faces = np.vstack((faces, tmp_stick_faces)) # wrap up the last segment tmp_spos = spos + (lsolid * direction + lspace * direction) * nstick tmp_epos = tmp_spos + lsolid * direction final_length, _ = rm.unit_vector(tmp_epos - spos, toggle_length=True) if final_length > length: tmp_epos = epos tmp_stick = gen_stick(spos=tmp_spos, epos=tmp_epos, thickness=thickness, type=sticktype, sections=sections) tmp_stick_faces = tmp_stick.faces + len(vertices) vertices = np.vstack((vertices, tmp_stick.vertices)) faces = np.vstack((faces, tmp_stick_faces)) return trm.Trimesh(vertices=vertices, faces=faces)
def gen_ellipsoid(pos=np.array([0, 0, 0]), axmat=np.eye(3), subdivisions=5): """ :param pos: :param axmat: 3x3 mat, each column is an axis of the ellipse :param subdivisions: levels of icosphere discretization :return: author: weiwei date: 20191228osaka """ sphere = tp.Sphere(sphere_radius=1, sphere_center=np.zeros(3), subdivisions=subdivisions) vertices = axmat.dot(sphere.vertices.T).T vertices = vertices + pos return trm.Trimesh(vertices=vertices, faces=sphere.faces)
def gen_axis( pos=np.array([0, 0, 0]), rotmat=np.eye(3), length=0.1, thickness=0.005): """ :param spos: 1x3 nparray :param epos: 1x3 nparray :param thickness: 0.005 m by default :return: author: weiwei date: 20191228osaka """ directionx = rotmat[:, 0] directiony = rotmat[:, 1] directionz = rotmat[:, 2] # x endx = directionx * length stickx = gen_stick(spos=pos, epos=endx, thickness=thickness) capx = gen_cone(spos=endx, epos=endx + directionx * thickness * 4, radius=thickness) # y endy = directiony * length sticky = gen_stick(spos=pos, epos=endy, thickness=thickness) capy = gen_cone(spos=endy, epos=endy + directiony * thickness * 4, radius=thickness) # z endz = directionz * length stickz = gen_stick(spos=pos, epos=endz, thickness=thickness) capz = gen_cone(spos=endz, epos=endz + directionz * thickness * 4, radius=thickness) vertices = np.vstack((stickx.vertices, capx.vertices, sticky.vertices, capy.vertices, stickz.vertices, capz.vertices)) capxfaces = capx.faces + len(stickx.vertices) stickyfaces = sticky.faces + len(stickx.vertices) + len(capx.vertices) capyfaces = capy.faces + len(stickx.vertices) + len(capx.vertices) + len( sticky.vertices) stickzfaces = stickz.faces + len(stickx.vertices) + len( capx.vertices) + len(sticky.vertices) + len(capy.vertices) capzfaces = capz.faces + len(stickx.vertices) + len(capx.vertices) + len( sticky.vertices) + len(capy.vertices) + len(stickz.vertices) faces = np.vstack((stickx.faces, capxfaces, stickyfaces, capyfaces, stickzfaces, capzfaces)) return trm.Trimesh(vertices=vertices, faces=faces)
def drawanySingleSurface(base, vertices, color): ''' draw a surface using a calculated fourth point to creat a hull :param base: :param vertices: :param faces: :param color: :return: ''' # print("faces in plotsurface",faces) surface_vertices = vertices # surface = humath.centerPoftrangle(surface_vertices[0][:3], surface_vertices[1][:3], surface_vertices[2][:3]) surface = trimesh.Trimesh(surface_vertices) surface = surface.convex_hull surface = da.trimesh_to_nodepath(surface) surface.set_color(color) surface.reparentTo(base.render)
def drawSingleFaceSurface(self, base, vertices, faces, color): ''' draw a surface using a calculated fourth point to creat a hull :param base: :param vertices: :param faces: :param color: :return: ''' # print("faces in plotsurface",faces) surface_vertices = np.array([vertices[faces[0]], vertices[faces[1]], vertices[faces[2]]]) surface = humath.centerPoftrangle(surface_vertices[0], surface_vertices[1], surface_vertices[2]) surface = trimesh.Trimesh(surface) surface = surface.convex_hull surface = gm.GeometricModel(surface) surface.set_rgba(color) surface.attach_to(base)
def extract_subtrimesh(objtrm, face_id_list, offset_pos=np.zeros(3), offset_rotmat=np.eye(3)): """ :param objtrm: :param face_id_list: :param offset_pos: :param offset_rotmat: :return: author: weiwei date: 20210120 """ if not isinstance(face_id_list, list): face_id_list = [face_id_list] tmp_vertices = offset_rotmat.dot( objtrm.vertices[objtrm.faces[face_id_list].flatten()].T).T + offset_pos tmp_faces = np.array(range(len(tmp_vertices))).reshape(-1, 3) return trm.Trimesh(vertices=tmp_vertices, faces=tmp_faces)
def _mesh_from_domain_grid(domain_grid, vertices): domain_0, domain_1 = domain_grid nrow = domain_0.shape[0] ncol = domain_0.shape[1] faces = np.empty((0, 3)) for i in range(nrow - 1): urgt_pnt0 = np.arange(i * ncol, i * ncol + ncol - 1).T urgt_pnt1 = np.arange(i * ncol + 1 + ncol, i * ncol + ncol + ncol).T urgt_pnt2 = np.arange(i * ncol + 1, i * ncol + ncol).T faces = np.vstack( (faces, np.column_stack((urgt_pnt0, urgt_pnt2, urgt_pnt1)))) blft_pnt0 = np.arange(i * ncol, i * ncol + ncol - 1).T blft_pnt1 = np.arange(i * ncol + ncol, i * ncol + ncol + ncol - 1).T blft_pnt2 = np.arange(i * ncol + 1 + ncol, i * ncol + ncol + ncol).T faces = np.vstack( (faces, np.column_stack((blft_pnt0, blft_pnt2, blft_pnt1)))) return trm.Trimesh(vertices=vertices, faces=faces)
def get_org_convexhull(pcd, color=(1, 1, 1), transparency=1, toggledebug=False): """ create CollisionModel by pcd :param pcd: :param color: :param transparency: :return: CollisionModel """ convexhull = trimesh.Trimesh(vertices=pcd) convexhull = convexhull.convex_hull obj = cm.CollisionModel(initor=convexhull, type="ball") if toggledebug: obj.set_rgba(color[0], color[1], color[2], transparency) obj.reparentTo(base.render) obj.showlocalframe() return obj
capsule_1 = capsule_link_start_end(start=np.array([0, 0, 0]), end=np.array([0, 0, 0.1])) capsule_2 = capsule_link_start_end(start=np.array([0, 0, 0]), end=np.array([0, 0.2, 0])) a_vertices = capsule_1.objtrm.vertices a_face_normals = capsule_1.objtrm.face_normals a_vertex_normals = capsule_1.objtrm.face_normals a_faces = capsule_1.objtrm.faces capsule_1.objtrm.export("capsule_1.stl") capsule_2.objtrm.export("capsule_2.stl") capsule_3 = tb.union([capsule_1.objtrm, capsule_2.objtrm], engine="blender") capsule_3.export("capsule_3.stl") capsule_3_wrsmesh = cm.CollisionModel("capsule_3.stl").attach_to(base) a_sub = trimesh.Trimesh(p).convex_hull a_cm = cm.CollisionModel(a_sub) b_sub = trimesh.Trimesh(m).convex_hull b_cm = cm.CollisionModel(b_sub) # a_gm = gm.GeometricModel(a_sub).attach_to(base) print(a_sub) a_sub.export("a_sub.stl") b_sub.export("b_sub.stl") a_trimesh = tri.load("a_sub.stl") b_trimesh = tri.load("b_sub.stl") c = tb.union([a_trimesh, b_trimesh], engine="blender") c.export("c_sub.stl") # c_wrsmesh = cm.CollisionModel("c_sub.stl").attach_to(base) # c = gm.GeometricModel(c_wrsmesh).attach_to(base) # tel.export_stl_ascii(a_mesh)
def gen_circarrow(axis=np.array([1, 0, 0]), starting_vector=None, portion=0.3, center=np.array([0, 0, 0]), radius=0.005, thickness=0.0015, sections=8, discretization=24): """ :param axis: the circ arrow will rotate around this axis 1x3 nparray :param portion: 0.0~1.0 :param center: the center position of the circ 1x3 nparray :param radius: :param thickness: :param rgba: :param discretization: number sticks used for approximation :return: author: weiwei date: 20200602 """ unitaxis = rm.unit_vector(axis) if starting_vector is None: starting_vector = rm.orthogonal_vector(unitaxis) else: starting_vector = rm.unit_vector(starting_vector) starting_pos = starting_vector * radius + center discretizedangle = 2 * math.pi / discretization ndist = int(portion * discretization) # gen the last arrow first # gen the remaining torus if ndist > 0: lastpos = center + np.dot( rm.rotmat_from_axangle(unitaxis, (ndist - 1) * discretizedangle), starting_vector) * radius nxtpos = center + np.dot( rm.rotmat_from_axangle(unitaxis, ndist * discretizedangle), starting_vector) * radius arrow = gen_arrow(spos=lastpos, epos=nxtpos, thickness=thickness, sections=sections, sticktype="round") vertices = arrow.vertices faces = arrow.faces lastpos = starting_pos for i in range(1 * np.sign(ndist), ndist, 1 * np.sign(ndist)): nxtpos = center + np.dot( rm.rotmat_from_axangle(unitaxis, i * discretizedangle), starting_vector) * radius stick = gen_stick(spos=lastpos, epos=nxtpos, thickness=thickness, sections=sections, type="round") stickfaces = stick.faces + len(vertices) vertices = np.vstack((vertices, stick.vertices)) faces = np.vstack((faces, stickfaces)) lastpos = nxtpos return trm.Trimesh(vertices=vertices, faces=faces) else: return trm.Trimesh()