def test_deterministic(self):
s = Sampler()
samples = s.sample(5, 3, method=SampleMethod.deterministic_uniform)
assert samples.shape == (5, 3)
assert_in_range(samples.flatten())
desired_samples = np.array([
[0.33333333, 0.2, 0.14285714],
[0.66666667, 0.4, 0.28571429],
[0.11111111, 0.6, 0.42857143],
[0.44444444, 0.8, 0.57142857],
[0.77777778, 0.04, 0.71428571],
])
assert_almost_equal(samples, desired_samples)
# we should get different samples on a second run
desired_samples_2 = np.array([
[0.22222222, 0.24, 0.85714286],
[0.55555556, 0.44, 0.02040816],
[0.88888889, 0.64, 0.16326531],
[0.03703704, 0.84, 0.30612245],
[0.37037037, 0.08, 0.44897959],
])
samples_2 = s.sample(5, 3, method=SampleMethod.deterministic_uniform)
assert_almost_equal(samples_2, desired_samples_2)
def __init__(self, position, quaternion, tolerance: List[Tolerance]):
# assert len(position) == len(tolerance)
self.pos = np.array(position)
self.quat = quaternion
self.pos_tol = tolerance
self.tol = tolerance
self.sampler = Sampler()
self.sample_dim = super().count_tolerance(tolerance)
def __init__(self, position, quaternion, pos_tol: List[Tolerance],
rot_tol: List[Tolerance]):
self.pos = np.array(position)
self.quat = quaternion
self.pos_tol = pos_tol
self.rot_tol = rot_tol
self.tol = pos_tol + rot_tol
self.sample_dim = super().count_tolerance(self.tol)
self.sampler = Sampler()
def __init__(
self,
position,
quaternion,
pos_tol: List[Tolerance],
quat_tol: QuaternionTolerance,
):
self.pos = np.array(position)
self.quat = quaternion
self.pos_tol = pos_tol
self.quat_tol = quat_tol
self.tol = pos_tol + [quat_tol]
# TODO not using the sampler for the quaternions at the moment
self.sample_dim = self.count_tolerance(
self.pos_tol) # 1 uniform and 3 gaussian TODO
self.sampler = Sampler()
class TolEulerPt(Pose, PathPt):
def __init__(self, position, quaternion, pos_tol: List[Tolerance],
rot_tol: List[Tolerance]):
self.pos = np.array(position)
self.quat = quaternion
self.pos_tol = pos_tol
self.rot_tol = rot_tol
self.tol = pos_tol + rot_tol
self.sample_dim = super().count_tolerance(self.tol)
self.sampler = Sampler()
def to_transform(self, sample):
tf = np.eye(4)
tf[:3, 3] = sample[:3]
tf[:3, :3] = self.rotation_matrix @ rpy_to_rot_mat(sample[3:])
return tf
def transform_to_rel_tolerance_deviation(self, tf):
""" Convert tf in world frame to position expressed in local path frame. """
T_rel = tf_inverse(self.transformation_matrix) @ tf
rxyz = rotation_matrix_to_rpy(T_rel[:3, :3])
rxyz = np.array(rxyz)
return np.hstack((T_rel[:3, 3], rxyz))
def rel_to_abs(self, grid):
R = self.rotation_matrix
X_rel, Y_rel, Z_rel = R[:, 0], R[:, 1], R[:, 2]
samples = []
for r in grid:
dx, dy, dz = r[0], r[1], r[2]
sample = np.zeros(6)
sample[:3] = self.pos + dx * X_rel + dy * Y_rel + dz * Z_rel
sample[3:] = r[3:]
samples.append(sample)
return [self.to_transform(sample) for sample in samples]
def sample_grid(self):
tol_ranges = [tol.discretize() for tol in self.tol]
relative_grid = create_grid(tol_ranges)
return self.rel_to_abs(relative_grid)
def sample_incremental(self, num_samples, method: SampleMethod):
R = self.sampler.sample(num_samples, self.sample_dim, method)
# scale samples from range [0, 1] to desired range
relative_grid = np.zeros((num_samples, len(self.tol)))
cnt = 0
for i, value in enumerate(self.tol):
if value.has_tolerance:
relative_grid[:,
i] = (R[:, cnt] * (value.upper - value.lower) +
value.lower)
cnt += 1
else:
relative_grid[:, i] = np.zeros(num_samples)
return self.rel_to_abs(relative_grid)
def test_get_many_samples(self):
s = Sampler()
samples = s.sample(1000, 3, method=SampleMethod.deterministic_uniform)
assert len(samples) == 1000
def test_raise_exception(self):
s = Sampler()
with pytest.raises(Exception) as info:
s.sample(5, 3, method=3)
msg = "Unkown sampling method"
assert msg in str(info.value)
def test_random(self):
s = Sampler()
samples = s.sample(5, 3, method=SampleMethod.random_uniform)
assert samples.shape == (5, 3)
assert_in_range(samples.flatten())
class TolQuatPt(Pose, PathPt):
def __init__(
self,
position,
quaternion,
pos_tol: List[Tolerance],
quat_tol: QuaternionTolerance,
):
self.pos = np.array(position)
self.quat = quaternion
self.pos_tol = pos_tol
self.quat_tol = quat_tol
self.tol = pos_tol + [quat_tol]
# TODO not using the sampler for the quaternions at the moment
self.sample_dim = self.count_tolerance(
self.pos_tol) # 1 uniform and 3 gaussian TODO
self.sampler = Sampler()
def to_transform(self, pos_sample, quat_sample):
tf = np.eye(4)
tf[:3, 3] = pos_sample
tf[:3, :3] = quat_sample.rotation_matrix
return tf
def transform_to_rel_tolerance_deviation(self, tf):
""" Convert tf in world frame to position expressed in local path frame. """
T_rel = tf_inverse(self.transformation_matrix) @ tf
q = Quaternion(matrix=tf)
return np.hstack((T_rel[:3, 3], quat_distance(q, self.quat)))
def rel_to_abs(self, pos_grid):
R = self.rotation_matrix
X_rel, Y_rel, Z_rel = R[:, 0], R[:, 1], R[:, 2]
pos_samples = []
for r in pos_grid:
dx, dy, dz = r[0], r[1], r[2]
sample = self.pos + dx * X_rel + dy * Y_rel + dz * Z_rel
pos_samples.append(sample)
return pos_samples
def sample_grid(self):
raise NotImplementedError
def sample_incremental(self, num_samples, method: SampleMethod):
# sample position if toleranced
R = self.sampler.sample(num_samples, self.sample_dim, method)
# scale samples from range [0, 1] to desired range
relative_grid = np.zeros((num_samples, len(self.pos_tol)))
cnt = 0
for i, value in enumerate(self.pos_tol):
if value.has_tolerance:
relative_grid[:,
i] = (R[:, cnt] * (value.upper - value.lower) +
value.lower)
cnt += 1
else:
relative_grid[:, i] = np.zeros(num_samples)
pos_samples = self.rel_to_abs(relative_grid)
# sample orientation
if method == SampleMethod.random_uniform:
quat_samples = [
self.quat.random_near(self.quat_tol.dist)
for _ in range(num_samples)
]
else:
raise NotImplementedError
return [
self.to_transform(p, q) for p, q in zip(pos_samples, quat_samples)
]