def tagVisible(self, oMt, size):
""" It assumes that updateGeometryPlacements has been called """
idc = self.model.getFrameId("xtion_rgb_optical_frame")
camera = self.model.frames[idc]
oMc = self.data.oMf[idc]
# oMc.rotation[:,2] view axis
# oMt.rotation[:,2] normal of the tag, on the tag side.
if not isinstance(oMt, pinocchio.SE3):
oMt = pinocchio.XYZQUATToSE3(oMt)
cos_theta = -np.dot(oMc.rotation[:, 2], oMt.rotation[:, 2])
if cos_theta < self.cos_angle_thr:
return False
pts = self.tagToTetahedronPts(oMt, size)
tetahedron = hppfcl.BVHModelOBBRSS()
tetahedron.beginModel(4, 5)
tetahedron.addSubModel(pts, _tetahedron_tris)
tetahedron.endModel()
request = hppfcl.CollisionRequest()
Id = hppfcl.Transform3f()
for go, oMg in zip(self.gmodel.geometryObjects, self.gdata.oMg):
# Don't check for collision with the camera, except with the field_of_view
if go.parentJoint == camera.parent and go.name != "field_of_view":
continue
result = hppfcl.CollisionResult()
hppfcl.collide(go.geometry, hppfcl.Transform3f(oMg), tetahedron,
Id, request, result)
if result.isCollision():
return False
return True
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)
x_grid = np.linspace(0., 1., nx)
x_half_pad = 0.5 * (x_grid[1] - x_grid[0])
x_bins = np.digitize(X, x_grid + x_half_pad)
x_dim = x_grid[-1] - x_grid[0]
ny = 20
y_grid = np.linspace(0., 1., ny)
y_half_pad = 0.5 * (y_grid[1] - y_grid[0])
y_bins = np.digitize(Y, y_grid + y_half_pad)
y_dim = y_grid[-1] - y_grid[0]
point_bins = y_bins * nx + x_bins
heights = np.zeros((ny, nx))
np.maximum.at(heights.ravel(), point_bins, Z)
point_cloud = fcl.BVHModelOBBRSS()
point_cloud.beginModel(0, num_points)
point_cloud.addVertices(points.T)
height_field = fcl.HeightFieldOBBRSS(x_dim, y_dim, heights, min(Z))
height_field_placement = point_cloud_placement * pin.SE3(
np.eye(3),
0.5 * np.array([x_grid[0] + x_grid[-1], y_grid[0] + y_grid[-1], 0.]))
go_point_cloud = pin.GeometryObject("point_cloud", 0, point_cloud,
point_cloud_placement)
go_point_cloud.meshColor = np.ones((4))
collision_model.addGeometryObject(go_point_cloud)
visual_model.addGeometryObject(go_point_cloud)
go_height_field = pin.GeometryObject("height_field", 0, height_field,