def cylinder_link_start_end(start, end, radius = 0.0003):
start = start
end = end
height = np.linalg.norm(end - start, ord = 2, axis = 0, keepdims=True)[0]
vector = rm.unit_vector(end - start)
rot3 = rm.rotmat_between_vectors(np.array([0,0,1]),vector)
rot4 = rm.homomat_from_posrot(pos = start, rot = rot3)
cylinder = cm.gen_cylinder(radius=radius, height=height, section=30, homomat=rot4)
return cylinder
def gen_cone(spos=np.array([0, 0, 0]),
epos=np.array([0.1, 0, 0]),
radius=0.005,
sections=8):
"""
: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
:return:
author: weiwei
date: 20191228osaka
"""
height = np.linalg.norm(spos - epos)
pos = spos
rotmat = rm.rotmat_between_vectors(np.array([0, 0, 1]), epos - spos)
homomat = rm.homomat_from_posrot(pos, rotmat)
return tp.Cone(height=height,
radius=radius,
sections=sections,
homomat=homomat)
def gen_roundstick(spos=np.array([0, 0, 0]),
epos=np.array([0.1, 0, 0]),
thickness=0.005,
count=[8, 8]):
"""
:param spos:
:param epos:
:param thickness:
:return: a Trimesh object (Primitive)
author: weiwei
date: 20191228osaka
"""
pos = spos
height = np.linalg.norm(epos - spos)
if np.allclose(height, 0):
rotmat = np.eye(3)
else:
rotmat = rm.rotmat_between_vectors(np.array([0, 0, 1]), epos - spos)
homomat = rm.homomat_from_posrot(pos, rotmat)
return tp.Capsule(height=height,
radius=thickness / 2.0,
count=count,
homomat=homomat)
def gen_rectstick(spos=np.array([0, 0, 0]),
epos=np.array([0.1, 0, 0]),
thickness=.005,
sections=8):
"""
: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
:return: a Trimesh object (Primitive)
author: weiwei
date: 20191228osaka
"""
pos = spos
height = np.linalg.norm(epos - spos)
if np.allclose(height, 0):
rotmat = np.eye(3)
else:
rotmat = rm.rotmat_between_vectors(np.array([0, 0, 1]), epos - spos)
homomat = rm.homomat_from_posrot(pos, rotmat)
return tp.Cylinder(height=height,
radius=thickness / 2.0,
sections=sections,
homomat=homomat)