def obstacles_to_volume_2dpcd(geoms, n_pts):
points = geoms.compute_volume_pcd(int(1.5 * n_pts))
points[:, 2] = 0
# sparse representation
points, indices = utils.sparse_subset(points, 0.015)
# match required size
points, indices = utils.match_size(points, n_pts)
return points
def compute_volume_pcd(self, n_points):
if len(self.geom_objs) == 0:
return np.zeros((0, 3)), np.zeros((0, 3))
# buggy function
# points = trimesh.sample.volume_mesh(self.union_mesh, 6 * n_points)
ray_intersector, scene = self.ray_intersector()
rand_points = (np.random.random(
(6 * n_points, 3)) * scene.extents) + scene.bounds[0]
contains = ray_intersector.contains_points(rand_points)
points = rand_points[contains]
points, indices = utils.match_size(points, n_points)
return points
def obstacles_to_surface_2dpcd(geoms, n_pts):
points, normals = geoms.compute_surface_pcd(5 * n_pts)
# reject points with normals in the z direction or outside of [0, 1]
within_box = np.logical_and(points[:, :2] < 1.0,
points[:, :2] > 0.0).all(axis=1)
hori_normals = np.abs(normals[:, 2]) < 1e-3
valid_pts = np.logical_and(within_box, hori_normals)
points = points[valid_pts]
normals = normals[valid_pts]
points[:, 2] = 0
# sparse representation
points, indices = utils.sparse_subset(points, 0.015)
normals = normals[indices]
# match required size
points, indices = utils.match_size(points, n_pts)
normals = normals[indices]
return points, normals
def represent_obstacles(self, oMi, oMg):
ref = oMi[1]
ref_inv = ref.inverse()
rays_origins = self.witness_points(self.env.robot_name, oMi, oMg)
origins, rays = self.geoms.compute_origins_rays(rays_origins, self.rays)
points, normals, _ = self.geoms.ray_intersections(
self.ray_intersector, self.geoms_scene, origins, rays
)
# points in reference frame
points_ref = utils.apply_transformation(ref_inv, points)
# only keep points close to reference
norm_ref = np.linalg.norm(points_ref, axis=1)
close = np.where(norm_ref < self.visibility_radius)[0]
points = points[close]
points_ref = points_ref[close]
normals = normals[close]
normals_ref = normals.dot(ref_inv.rotation.T)
norm_ref = norm_ref[close]
# local
indices = np.argsort(norm_ref)
points_ref = points_ref[indices]
normals_ref = normals_ref[indices]
local_pcd = np.hstack((points_ref, normals_ref))
if local_pcd.shape[0] == 0:
local_pcd = np.zeros((1, 6))
sparse_pcd, indices = utils.sparse_subset(local_pcd, self.memory_distance)
obstacles_repr = sparse_pcd[: self.n_samples]
obstacles_repr = utils.match_size(sparse_pcd, self.n_samples)[0]
# uncomment for visualization
# points, normals = obstacles_repr[:, :3], obstacles_repr[:, 3:]
# self.env.o3d_viz.show_pcd(points, normals, blocking=False)
obstacles_repr[:, :3] = self.normalize(
obstacles_repr[:, :3], self.coordinate_frame
)
return obstacles_repr