def __init__(
self,
urdfString=None,
urdfFilename=None,
fov=np.radians((60., 90.)),
# fov = np.radians((49.5, 60)),
geoms=["arm_3_link_0"],
):
if isinstance(fov, str):
self.fov = fov
fov_fcl = hppfcl.MeshLoader().load(
hpp.rostools.retrieve_resource(fov))
else:
# build FOV tetahedron
dx, dy = np.sin(fov)
pts = hppfcl.StdVec_Vec3f()
self.fov = [
(0, 0, 0),
(dx, dy, 1),
(-dx, dy, 1),
(-dx, -dy, 1),
(dx, -dy, 1),
]
pts.extend([np.array(e) for e in self.fov])
self.fov.append(self.fov[1])
fov_fcl = hppfcl.BVHModelOBBRSS()
fov_fcl.beginModel(4, 5)
fov_fcl.addSubModel(pts, _tetahedron_tris)
fov_fcl.endModel()
self.cos_angle_thr = np.cos(np.radians(70))
if urdfFilename is None:
assert urdfString is not None
# Pinocchio does not allow to build a GeometryModel from a XML string.
urdfFilename = '/tmp/tmp.urdf'
with open(urdfFilename, 'w') as f:
f.write(urdfString)
self.model, self.gmodel = pinocchio.buildModelsFromUrdf(
hpp.rostools.retrieve_resource(urdfFilename),
root_joint=pinocchio.JointModelPlanar(),
geometry_types=pinocchio.GeometryType.COLLISION)
id_parent_frame = self.model.getFrameId("xtion_rgb_optical_frame")
parent_frame = self.model.frames[id_parent_frame]
go = pinocchio.GeometryObject("field_of_view", id_parent_frame,
parent_frame.parent, fov_fcl,
parent_frame.placement)
self.gid_field_of_view = self.gmodel.addGeometryObject(go, self.model)
for n in geoms:
assert self.gmodel.existGeometryName(n)
self.gmodel.addCollisionPair(
pinocchio.CollisionPair(self.gmodel.getGeometryId(n),
self.gid_field_of_view))
self.data = pinocchio.Data(self.model)
self.gdata = pinocchio.GeometryData(self.gmodel)
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 tagToTetahedronPts(self, oMt, size, margin = 0.002):
""" It assumes that updateGeometryPlacements has been called """
if not isinstance(oMt, pinocchio.SE3):
oMt = pinocchio.XYZQUATToSE3(oMt)
pts = hppfcl.StdVec_Vec3f()
idc = self.model.getFrameId("xtion_rgb_optical_frame")
C = self.data.oMf[idc].translation + 0.002*self.data.oMf[idc].rotation[:,2]
pts.append(C)
for pt in [
np.array(( size / 2, size / 2, 0)),
np.array((-size / 2, size / 2, 0)),
np.array((-size / 2, -size / 2, 0)),
np.array(( size / 2, -size / 2, 0)), ]:
P = oMt * pt
u = (C-P)
u /= np.linalg.norm(u)
pts.append(P + margin * u)
return pts
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.")
