swangnice/rotor_py_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added fast 2D range sensor for navigation/estimation.

Browse Source
This commit is contained in:
spencerfolk
2024-07-26 17:48:24 -04:00
parent 37ccd4a70f
commit fa21ebd717
1 changed files with 265 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

265
rotorpy/sensors/range_sensors.py Normal file
View File

@@ -0,0 +1,265 @@
import numpy as np
from scipy.spatial.transform import Rotation
import copy
from rotorpy.utils.occupancy_map import OccupancyMap
def edges_from_extents_2d(extents):
"""
Return the 4 edges of the rectangle given by the extents.
Inputs:
extents: the array (xmin, xmax, ymin, ymax) that describe the rectangle of interest.
Outputs:
edges: a list of vertices [[(a0.x, a0.y), (b0.x, b0.y)]
[(a1.x, a1.y), (b1.x, b1.y)]
.....
]
that describe the edges of the rectangle give by extents.
(xmin,ymax)--------(2)---------(xmax,ymax)
| |
| (1)
(3) |
| |
(xmin,ymin)--------(0)---------(xmax,ymin)
"""
xmin = extents[0]
xmax = extents[1]
ymin = extents[2]
ymax = extents[3]
edges = [(xmin, ymin, xmax, ymin),
(xmax, ymin, xmax, ymax),
(xmax, ymax, xmin, ymax),
(xmin, ymax, xmin, ymin)]
return edges
class TwoDRangeSensor():
"""
The 2D Range sensor will provide distance measurements around the UAV on the XY plane (top-down).
The sensor is placed at the CoM of the UAV, and casts rays from the CoM outward toward objects. The
distance between objects and the UAV, as measured by the i'th ray, is given in the i'th row of the
sensor output. The distance measurement is clipped by Dmin and Dmax, bounds that are specified by
the user (with defaults of course). The distance measurement can also be optionally corrupted by
white noise using the specified noise density term. The angular resolution specified will determine
the number of rays casted.
"""
def __init__(self, world, sampling_rate,
map_resolution = 1, # The resolution of the occupancy grid used for generating measurements, in meters.
angular_resolution=10, # The angular resolution, measured in degrees.
angular_fov=360, # Angular field of view, this determines the width of the region with which rays are cast, relative to the sensor heading, degrees.
Dmin=0.025, # The minimum distance measurable by the range sensor, m.
Dmax=100, # The maximum distance measurable by the range sensor, m.
noise_density=0.0, # The noise density of the white noise added to the range sensor, [m / sqrt(Hz)].
fixed_heading=True, # If true, the heading of the sensor will be world-fixed; if false, the rays will be cast relative to the robot's heading
):
self.world = world
self.occ_map = OccupancyMap(world=self.world, resolution=(map_resolution, map_resolution, map_resolution), margin=0)
self.rate_scale = np.sqrt(sampling_rate/2)
self.angular_resolution = angular_resolution
self.angular_fov = angular_fov
self.Dmin = Dmin
self.Dmax = Dmax
self.sensor_variance = noise_density
self.fixed_heading = fixed_heading
# First, construct an array of ray directions that we will iterate over.
self.ray_angles = np.arange(-self.angular_fov/2, self.angular_fov/2+self.angular_resolution, step=self.angular_resolution) # Centered at 0
self.N_rays = len(self.ray_angles) # save the number of rays for later looping
self.set_ray_vectors(heading=0)
# Next save relevant quantities for each physical block in the map
self.world_edges = []
world_slopes = []
for block in self.world.world['blocks']:
extents = block['extents'] # Get the extents of the block
edges = edges_from_extents_2d(extents) # Extract the four edges of each block
self.world_edges.append(edges)
return
def set_ray_vectors(self, heading=0):
"""
Set the ray vectors array based on the current heading.
Inputs:
heading: the current heading angle in degrees.
"""
rotated_ray_angles = self.ray_angles + heading
x_rays = np.cos(rotated_ray_angles*np.pi/180).reshape(-1,1)
y_rays = np.sin(rotated_ray_angles*np.pi/180).reshape(-1,1)
self.ray_vectors = np.hstack((x_rays, y_rays))
return
def cast_rays(self, x, y):
"""
Cast rays from (x,y) and return the distances of the point (x,y) to the nearest obstacles in the map.
Raycasting is implemented using the algorithm described here: https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
Inputs:
x: the x position of the point of interest
y: the y position of the point of interest
Outputs:
d: an array of distances that is of shape (self.N_rays,)
"""
d = np.zeros((self.N_rays,))
# For ray casting we'll have to use (x,y) as one of our points for all rays. Do this now to be more efficient
x3 = x
y3 = y
# Loop over each object
distances = np.ones((self.N_rays,))*1e5 # Begin with
for object_edges in self.world_edges:
# Object edges is a list of tuples, each tuple describing one of the four edges
for edge in object_edges:
# First collect the coordinates for point 1 and 2 corresponding to this edge
x1 = edge[0]
y1 = edge[1]
x2 = edge[2]
y2 = edge[3]
# Using self.ray_vectors and the current position (x,y), create the ray's other point (x4,y4)
# The rays are of length Dmax.
x4 = x + self.Dmax*self.ray_vectors[:,0] # This gives us an array of x coordinates for each ray
y4 = y + self.Dmax*self.ray_vectors[:,1] # This gives us an array of y coordinates for each ray
# Compute the denominator of the t-u test, which will be used to check for intersections
den = (x1 - x2)*(y3 - y4) - (y1 - y2)*(x3 - x4)
# Assign nan to any instances where the denominator equals 0, because this indicates that the ray and boundary are parallel
den[den == 0] = np.nan
# Compute t
t = ((x1 - x3)*(y3 - y4) - (y1 - y3)*(x3 - x4))/den
# Compute u
u = ((x1 - x3)*(y1 - y2) - (y1 - y3)*(x1 - x2))/den
# Now do the check to see if these lines intersect and compute the intersection point
intersected = (t > 0)*(t < 1)*(u > 0)*1.
intersected[intersected==0] = np.nan
# Compute the distance away from the point using u and (x,y)
dx = u*(x4-x3)
dy = u*(y4-y3)
# Now that we have (x_int, y_int), compute the distance away from (x,y) for those that are intersected
object_distances = np.sqrt(dx**2 + dy**2)*intersected
# If any of these distances are less than the current distance saved in distances, rewrite that measurement
distances[object_distances < distances] = object_distances[object_distances < distances]
return distances
def measurement(self, state, with_noise=True):
"""
Computes and returns the measurement at the current time step.
Inputs:
state, a dict describing the state with keys
x, position, m, shape=(3,)
v, linear velocity, m/s, shape=(3,)
q, quaternion [i,j,k,w], shape=(4,)
w, angular velocity (in LOCAL frame!), rad/s, shape=(3,)
with_noise, a boolean to indicate if noise is added
Outputs:
ranges, an array that is
"""
pos = state['x']
# First update the ray angles and recompute the ray vectors if the heading should follow the robot
if not self.fixed_heading:
orientation = Rotation.from_quat(state['q']).as_euler('zyx', degrees=True)
heading = orientation[0] # Extract the yaw angle in degrees of the robot.
self.set_ray_vectors(heading=heading) # This sets self.ray_vectors appropriately
# Cast the rays and compute the ranges for each ray
self.ranges = np.clip(self.cast_rays(pos[0], pos[1]), self.Dmin, self.Dmax)
if with_noise:
self.ranges += self.rate_scale * np.random.normal(scale=np.abs(self.sensor_variance), size=self.N_rays)
return self.ranges
if __name__=="__main__":
import matplotlib.pyplot as plt
from rotorpy.utils.generate_maps import plot_map
import matplotlib.colors as mcolors
from matplotlib.patches import Rectangle
import os
fig_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'data_out')
random_color = np.random.uniform(low=0, high=1, size=(100, 3))
def plot_range(axes, world, pos, ranges, range_sensor, detected_edges=None):
axes[0].plot(pos[0], pos[1], 'ro', zorder=10, label="Robot Position")
if detected_edges is None:
axes[0].plot((pos[0:2] + ranges[:,np.newaxis]*range_sensor.ray_vectors)[:,0], (pos[0:2] + ranges[:,np.newaxis]*range_sensor.ray_vectors)[:,1], 'ko', markersize=3, label="Ray Intersection Points")
else:
for (i,edge) in enumerate(detected_edges):
axes[0].plot((pos[0:2] + ranges[edge,np.newaxis]*range_sensor.ray_vectors[edge])[:,0], (pos[0:2] + ranges[edge,np.newaxis]*range_sensor.ray_vectors[edge])[:,1], color=random_color[i], marker='o', markersize=3, linestyle='none', label="Ray Intersection Points")
line_objects = []
for i in range(range_sensor.N_rays):
xvals = [pos[0], (pos[0:2] + ranges[:,np.newaxis]*range_sensor.ray_vectors)[i,0]]
yvals = [pos[1], (pos[0:2] + ranges[:,np.newaxis]*range_sensor.ray_vectors)[i,1]]
line = axes[0].plot(xvals, yvals, 'k-', linewidth=0.5, alpha=0.5)
if detected_edges is None:
axes[1].plot(range_sensor.ray_angles, range_sensor.ranges, 'r.', linewidth=1.5)
else:
axes[1].plot(range_sensor.ray_angles, range_sensor.ranges, 'k.', linewidth=1.5, alpha=0.1)
for (i,edge) in enumerate(detected_edges):
axes[1].plot(range_sensor.ray_angles[edge], range_sensor.ranges[edge], color=random_color[i], linestyle='none', marker='.')
axes[1].plot([-180, 180], [range_sensor.Dmin, range_sensor.Dmin], 'k--', linewidth=1)
axes[1].plot([-180, 180], [range_sensor.Dmax, range_sensor.Dmax], 'k--', linewidth=1)
return
# Choose a map
# Load the map from the world directory
from rotorpy.world import World
map_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'worlds')
maps = [m for m in os.listdir(map_dir) if '.json' in m]
world = World.grid_forest(n_rows=10, n_cols=10, width=1, height=10, spacing=5)
# Sensor intrinsics
angular_fov = 360
angular_resolution = 1
fixed_heading = True
noise_density = 0.005
# Create sensor
range_sensor = TwoDRangeSensor(world, sampling_rate=100, angular_fov=angular_fov, angular_resolution=angular_resolution, fixed_heading=fixed_heading, noise_density=noise_density)
(xmin, xmax, ymin, ymax, zmin, zmax) = world.world['bounds']['extents']
pos0 = np.array([0.5*(xmax+xmin), 0.5*(ymax+ymin), 0.5*(zmax+zmin)])
state = {'x': pos0, 'q': np.array([0, 0, 0.3826834, 0.9238795])}
ranges = range_sensor.measurement(state)
# Plotting
(fig, axes) = plt.subplots(nrows=1, ncols=2, num="Ray Intersections Test")
axes[1].set_xlabel("Ray Angle (deg)")
axes[1].set_ylabel("Range (m)")
axes[1].set_xlim([-190, 190])
plot_map(axes[0], world.world)
plot_range(axes, world, pos0, ranges, range_sensor)
plt.show()