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)