Code: Select all
ground_idxs = np.argwhere(grid_map[0:1,:] >= 50)
num_ground_idxs = ground_idxs.shape[0]
shapeTypes_ground = [p.GEOM_BOX for _ in range(num_ground_idxs)]
half_size = [resolution/2, resolution/2, resolution/2]
halfExtents_ground = [half_size for _ in range(num_ground_idxs)]
visualFramePositions_ground =\
(np.flip(ground_idxs,axis=1) + 0.5) * resolution + np.array(bbx_min)
visualGroundId = p.createVisualShapeArray(
shapeTypes = shapeTypes_ground,
halfExtents = halfExtents_ground,
visualFramePositions = visualFramePositions_ground.tolist()
)
mb_id = p.createMultiBody(
baseMass = 0,
baseVisualShapeIndex = visualGroundId,
basePosition = [0,0,0]
)
p.changeVisualShape(mb_id, -1, rgbaColor=[0, 0.5, 0, 0.5])
Code: Select all
np.argwhere(grid_map[0:1,:] >= 50)
Thanks~