in safety_gym/envs/engine.py [0:0]
def obs_lidar_pseudo(self, positions):
'''
Return a robot-centric lidar observation of a list of positions.
Lidar is a set of bins around the robot (divided evenly in a circle).
The detection directions are exclusive and exhaustive for a full 360 view.
Each bin reads 0 if there are no objects in that direction.
If there are multiple objects, the distance to the closest one is used.
Otherwise the bin reads the fraction of the distance towards the robot.
E.g. if the object is 90% of lidar_max_dist away, the bin will read 0.1,
and if the object is 10% of lidar_max_dist away, the bin will read 0.9.
(The reading can be thought of as "closeness" or inverse distance)
This encoding has some desirable properties:
- bins read 0 when empty
- bins smoothly increase as objects get close
- maximum reading is 1.0 (where the object overlaps the robot)
- close objects occlude far objects
- constant size observation with variable numbers of objects
'''
obs = np.zeros(self.lidar_num_bins)
for pos in positions:
pos = np.asarray(pos)
if pos.shape == (3,):
pos = pos[:2] # Truncate Z coordinate
z = np.complex(*self.ego_xy(pos)) # X, Y as real, imaginary components
dist = np.abs(z)
angle = np.angle(z) % (np.pi * 2)
bin_size = (np.pi * 2) / self.lidar_num_bins
bin = int(angle / bin_size)
bin_angle = bin_size * bin
if self.lidar_max_dist is None:
sensor = np.exp(-self.lidar_exp_gain * dist)
else:
sensor = max(0, self.lidar_max_dist - dist) / self.lidar_max_dist
obs[bin] = max(obs[bin], sensor)
# Aliasing
if self.lidar_alias:
alias = (angle - bin_angle) / bin_size
assert 0 <= alias <= 1, f'bad alias {alias}, dist {dist}, angle {angle}, bin {bin}'
bin_plus = (bin + 1) % self.lidar_num_bins
bin_minus = (bin - 1) % self.lidar_num_bins
obs[bin_plus] = max(obs[bin_plus], alias * sensor)
obs[bin_minus] = max(obs[bin_minus], (1 - alias) * sensor)
return obs