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