def __init__(self,
offset,
prec,
rob,
unknown,
decay,
bounds=None,
resolution=None,
cov_offset=1.0,
a_coeff=1.0,
r_coeff=1.0,
prior=0.5,
num_angles=1,
detect_range=np.Inf):
self.robot = copy.deepcopy(rob)
self.offset = np.asarray(offset)
if (bounds == None):
bounds = (-5.0, 5.0, -5.0, 5.0)
if (resolution == None):
resolution = 0.4
self.map = OccupancyGrid(bounds=bounds,
resolution=resolution,
unknown=unknown,
decay=decay,
prior=prior,
num_angles=num_angles)
self.prec = prec
self.cov_offset = cov_offset
self.a_coeff = a_coeff
self.r_coeff = r_coeff
self.num_angles = num_angles
self.detect_range = detect_range
def __init__(self, offset, prec, rob, unknown, decay, bounds=None,
resolution=None, cov_offset = 1.0, a_coeff=1.0, r_coeff=1.0,
prior=0.5, num_angles=1, detect_range=np.Inf):
self.robot = copy.deepcopy(rob)
self.offset = np.asarray(offset)
if (bounds == None):
bounds = (-5.0, 5.0, -5.0, 5.0)
if (resolution == None):
resolution = 0.4
self.map = OccupancyGrid(bounds=bounds, resolution=resolution,
unknown=unknown, decay=decay, prior=prior,
num_angles=num_angles)
self.prec = prec
self.cov_offset = cov_offset
self.a_coeff = a_coeff
self.r_coeff = r_coeff
self.num_angles = num_angles
self.detect_range = detect_range
class UltrasoundRobot(part_utils.ParticleSmoothingBaseRB):
""" Robot using ultrasonic distance measurement for navigation """
def __init__(self, offset, prec, rob, unknown, decay, bounds=None,
resolution=None, cov_offset = 1.0, a_coeff=1.0, r_coeff=1.0,
prior=0.5, num_angles=1, detect_range=np.Inf):
self.robot = copy.deepcopy(rob)
self.offset = np.asarray(offset)
if (bounds == None):
bounds = (-5.0, 5.0, -5.0, 5.0)
if (resolution == None):
resolution = 0.4
self.map = OccupancyGrid(bounds=bounds, resolution=resolution,
unknown=unknown, decay=decay, prior=prior,
num_angles=num_angles)
self.prec = prec
self.cov_offset = cov_offset
self.a_coeff = a_coeff
self.r_coeff = r_coeff
self.num_angles = num_angles
self.detect_range = detect_range
def update(self, u):
""" Move robot according to kinematics """
self.robot.update(u)
self.map.time_update()
def sample_input_noise(self, u):
return self.robot.sample_input_noise(u)
def next_pdf(self, next_state, u):
return self.robot.next_pdf(next_state, u)
def fwd_peak_density(self, u):
return self.robot.fwd_peak_density(u)
def calc_fov(self, dist):
# Add offset of sensor, offset is specified in local coordinate system,
# rotate to match global coordinates
pos = self.robot.get_pos()
theta = pos[2]
rot_mat = np.array(((math.cos(theta), -math.sin(theta)),
(math.sin(theta), math.cos(theta))))
pos[:2] = pos[:2] + rot_mat.dot(self.offset[:2])
# We can just add the angle, since unwrapping of
# angles is handled inside FOVCone
pos[2] = pos[2] + self.offset[2]
cell_diag = 1.44*self.map.resolution
# Limit max distance of updates
cone = sonar.FOVCone(pos[:2], pos[2], self.prec, min(dist+cell_diag, 15.0))
# To improve performance we just extract which cells are within
# the FOV once, and reause this both for measure and update.
cells = self.map.find_visible_cells(cone)
return cells
def convert_measurement(self, cells, dist):
""" Process new distance measurement """
if (cells != None):
# If outside "detect_range" don't detect anything, used update
# the empty area in front
# Create probability field for measurement
sonar_measure = sonar.SonarMeasurement(cells, dist, self.map.resolution/2,
r_coeff=self.r_coeff,
a_coeff=self.a_coeff,
cov_offset=self.cov_offset,
num_angles=self.num_angles,
norm=False)
# Eval measurement against map and update
return sonar_measure
else:
return None
def set_sensor_angle(self, theta):
""" Move sensor to specified angle """
self.offset[2] = theta
def sample_smooth(self, filt_traj, ind, next_cpart):
P = self.map.kf.P.ravel()
z_est = self.map.kf.x_new.ravel()
Q = self.map.kf.Q.ravel()
H = P/(Q+P)
Pi = P - H*P
u = z_est + H*(next_cpart.z-z_est)
cpart = self.collapse()
# cpart.z = np.random.normal(u,Pi)
cpart.z = u.ravel()
return cpart
def measure(self,y):
a = y[0]
dist = y[1]
self.set_sensor_angle(a)
cells = self.calc_fov(dist)
if (cells != None):
prob = self.map.eval_measurement(cells, dist)
sm = self.convert_measurement(cells, dist)
if (sm):
self.map(sm)
return math.log(prob)
else:
return -np.Inf
def collapse(self):
""" Return a sample of the particle where the linear states
are drawn from the MVN that results from CLGSS structure """
return UltrasoundRobotCollapsed(self)
def clin_update(self, u):
map = copy.deepcopy(self.map)
map.time_update()
return map
def clin_measure(self, y):
a = y[0]
dist = y[1]
self.set_sensor_angle(a)
cells = self.calc_fov(dist)
sm = self.convert_measurement(cells, dist)
map = copy.deepcopy(self.map)
if (sm):
map(sm)
return map
def clin_smooth(self, z_next, u):
map = copy.deepcopy(self.map)
map.smooth(z_next)
return map
def linear_input(self, u):
# Not used (in clin_update)
return None
def set_nonlin_state(self, eta):
self.robot.set_state(eta[0:7])
def get_nonlin_state(self):
return self.robot.get_state()
def set_lin_est(self, lest):
self.map = lest
def get_lin_est(self):
return self.map
class UltrasoundRobot(part_utils.ParticleSmoothingBaseRB):
""" Robot using ultrasonic distance measurement for navigation """
def __init__(self,
offset,
prec,
rob,
unknown,
decay,
bounds=None,
resolution=None,
cov_offset=1.0,
a_coeff=1.0,
r_coeff=1.0,
prior=0.5,
num_angles=1,
detect_range=np.Inf):
self.robot = copy.deepcopy(rob)
self.offset = np.asarray(offset)
if (bounds == None):
bounds = (-5.0, 5.0, -5.0, 5.0)
if (resolution == None):
resolution = 0.4
self.map = OccupancyGrid(bounds=bounds,
resolution=resolution,
unknown=unknown,
decay=decay,
prior=prior,
num_angles=num_angles)
self.prec = prec
self.cov_offset = cov_offset
self.a_coeff = a_coeff
self.r_coeff = r_coeff
self.num_angles = num_angles
self.detect_range = detect_range
def update(self, u):
""" Move robot according to kinematics """
self.robot.update(u)
self.map.time_update()
def sample_input_noise(self, u):
return self.robot.sample_input_noise(u)
def next_pdf(self, next_state, u):
return self.robot.next_pdf(next_state, u)
def fwd_peak_density(self, u):
return self.robot.fwd_peak_density(u)
def calc_fov(self, dist):
# Add offset of sensor, offset is specified in local coordinate system,
# rotate to match global coordinates
pos = self.robot.get_pos()
theta = pos[2]
rot_mat = np.array(
((math.cos(theta), -math.sin(theta)), (math.sin(theta),
math.cos(theta))))
pos[:2] = pos[:2] + rot_mat.dot(self.offset[:2])
# We can just add the angle, since unwrapping of
# angles is handled inside FOVCone
pos[2] = pos[2] + self.offset[2]
cell_diag = 1.44 * self.map.resolution
# Limit max distance of updates
cone = sonar.FOVCone(pos[:2], pos[2], self.prec,
min(dist + cell_diag, 15.0))
# To improve performance we just extract which cells are within
# the FOV once, and reause this both for measure and update.
cells = self.map.find_visible_cells(cone)
return cells
def convert_measurement(self, cells, dist):
""" Process new distance measurement """
if (cells != None):
# If outside "detect_range" don't detect anything, used update
# the empty area in front
# Create probability field for measurement
sonar_measure = sonar.SonarMeasurement(cells,
dist,
self.map.resolution / 2,
r_coeff=self.r_coeff,
a_coeff=self.a_coeff,
cov_offset=self.cov_offset,
num_angles=self.num_angles,
norm=False)
# Eval measurement against map and update
return sonar_measure
else:
return None
def set_sensor_angle(self, theta):
""" Move sensor to specified angle """
self.offset[2] = theta
def sample_smooth(self, filt_traj, ind, next_cpart):
P = self.map.kf.P.ravel()
z_est = self.map.kf.x_new.ravel()
Q = self.map.kf.Q.ravel()
H = P / (Q + P)
Pi = P - H * P
u = z_est + H * (next_cpart.z - z_est)
cpart = self.collapse()
# cpart.z = np.random.normal(u,Pi)
cpart.z = u.ravel()
return cpart
def measure(self, y):
a = y[0]
dist = y[1]
self.set_sensor_angle(a)
cells = self.calc_fov(dist)
if (cells != None):
prob = self.map.eval_measurement(cells, dist)
sm = self.convert_measurement(cells, dist)
if (sm):
self.map(sm)
return math.log(prob)
else:
return -np.Inf
def collapse(self):
""" Return a sample of the particle where the linear states
are drawn from the MVN that results from CLGSS structure """
return UltrasoundRobotCollapsed(self)
def clin_update(self, u):
map = copy.deepcopy(self.map)
map.time_update()
return map
def clin_measure(self, y):
a = y[0]
dist = y[1]
self.set_sensor_angle(a)
cells = self.calc_fov(dist)
sm = self.convert_measurement(cells, dist)
map = copy.deepcopy(self.map)
if (sm):
map(sm)
return map
def clin_smooth(self, z_next, u):
map = copy.deepcopy(self.map)
map.smooth(z_next)
return map
def linear_input(self, u):
# Not used (in clin_update)
return None
def set_nonlin_state(self, eta):
self.robot.set_state(eta[0:7])
def get_nonlin_state(self):
return self.robot.get_state()
def set_lin_est(self, lest):
self.map = lest
def get_lin_est(self):
return self.map
