def tetahedron():
pts = hppfcl.StdVec_Vec3f()
pts.append( np.array((0, 0, 0)) )
pts.append( np.array((0, 1, 0)) )
pts.append( np.array((1, 0, 0)) )
pts.append( np.array((0, 0, 1)) )
tri = hppfcl.StdVec_Triangle()
tri.append(hppfcl.Triangle(0,1,2))
tri.append(hppfcl.Triangle(0,1,3))
tri.append(hppfcl.Triangle(0,2,3))
tri.append(hppfcl.Triangle(1,2,3))
return hppfcl.Convex(pts, tri)
def create_bvh(self, mesh):
v = mesh.verts
f = mesh.faces
mesh_verts = np.array([[v[i], v[i + 1], v[i + 2]]
for i in range(0, len(v), 3)])
mesh_faces = [(int(f[i]), int(f[i + 1]), int(f[i + 2]))
for i in range(0, len(f), 3)]
bvh = hppfcl.BVHModelOBB()
bvh.beginModel(len(mesh_faces), len(mesh_verts))
vertices = hppfcl.StdVec_Vec3f()
[vertices.append(v) for v in mesh_verts]
triangles = hppfcl.StdVec_Triangle()
[
triangles.append(hppfcl.Triangle(f[0], f[1], f[2]))
for f in mesh_faces
]
bvh.addSubModel(vertices, triangles)
bvh.endModel()
return bvh
def test_convex(self):
verts = hppfcl.StdVec_Vec3f ()
faces = hppfcl.StdVec_Triangle ()
verts.extend( [ np.array([0, 0, 0]), np.array([0, 1, 0]), np.array([1, 0, 0]), ])
faces.append(hppfcl.Triangle(0,1,2))
convex = hppfcl.Convex(verts, faces)
verts.append (np.array([0, 0, 1]))
try:
convexHull = hppfcl.Convex.convexHull(verts, False, None)
qhullAvailable = True
except Exception as e:
self.assertEqual(str(e), "Library built without qhull. Cannot build object of this type.")
qhullAvailable = False
if qhullAvailable:
convexHull = hppfcl.Convex.convexHull(verts, False, "")
convexHull = hppfcl.Convex.convexHull(verts, True, "")
try:
convexHull = hppfcl.Convex.convexHull(verts[:3], False, None)
except Exception as e:
self.assertIn(str(e), "You shouldn't use this function with less than 4 points.")
def _add_fov_to_gui(gui, name, filename_or_pts, group=None, color=None):
if isinstance(filename_or_pts, str):
gui.addMesh(name, filename_or_pts)
if color is not None: gui.setColor(name, [0.1, 0.1, 0.9, 0.2])
else:
assert color is not None
gui.addCurve(name, filename_or_pts, color)
gui.setCurveMode(name, "TRIANGLE_FAN")
if group is not None: gui.addToGroup(name, group)
gui.setBoolProperty(name, "Selectable", False)
_tetahedron_tris = hppfcl.StdVec_Triangle()
_tetahedron_tris.append(hppfcl.Triangle(0, 1, 2))
_tetahedron_tris.append(hppfcl.Triangle(0, 2, 3))
_tetahedron_tris.append(hppfcl.Triangle(0, 3, 4))
_tetahedron_tris.append(hppfcl.Triangle(0, 4, 1))
class TiagoFOV:
def __init__(
self,
urdfString=None,
urdfFilename=None,
fov=np.radians((49.5, 60)),
geoms=["arm_3_link_0"],
):
if isinstance(fov, str):
self.fov = fov