def circle_point(self, point, radius, granularity=8, theta_offset=0):
'''
Circle a point whilst looking at it
This produces a generator, so for use:
circle = navigator.move.circle_point([1,2,0], 5)
for p in circle:
yield p.go()
'''
point = np.array(point)
angle_incrment = 2 * np.pi / granularity
sprinkles = transformations.euler_matrix(0, 0, angle_incrment)[:3, :3]
# Find first point to go to using boat rotation
next_point = np.append(normalize(self.nav.pose[0][:2] - point[:2]),
0) # Doing this in 2d
for i in range(granularity + 1):
new = point + radius * next_point
yield self.set_position(new).look_at(point).yaw_left(
theta_offset
) # Remember this is a generator - not a twisted yield
next_point = sprinkles.dot(next_point)
yield self.set_position(new).look_at(point).yaw_left(theta_offset)
def d_spiral_point(self,
point,
radius,
granularity=8,
revolutions=1,
direction='ccw',
theta_offset=0,
meters_per_rev=0):
"""
Sprials a point using discrete moves
This produces a generator
"""
point = np.array(point)
if direction == 'ccw':
angle_incrment = 2 * np.pi / granularity
else:
angle_incrment = -2 * np.pi / granularity
sprinkles = transformations.euler_matrix(0, 0, angle_incrment)[:3, :3]
# Find first point to go to using boat rotation
next_point = np.append(normalize(self.position[:2] - point[:2]),
0) # Doing this in 2d
radius_increment = meters_per_rev / granularity
for i in range(granularity * revolutions + 1):
new = point + radius * next_point
radius += radius_increment
yield self.set_position(new).look_at(point).yaw_left(theta_offset)
next_point = sprinkles.dot(next_point)
yield self.set_position(new).look_at(point).yaw_left(theta_offset)
def test_normalize_vector(self):
'''Test vector normalization'''
for k in range(10):
rand_vec = np.random.random(k) # Make a random k-length vector
# Ignore the astronomically unlikely case that a vector has near 0 norm
if not np.isclose(np.sum(rand_vec), 0):
normalized = normalize(rand_vec)
norm = np.linalg.norm(normalized)
# Test that the norm is 1
np.testing.assert_almost_equal(norm, 1.0, err_msg="The normalized vector did not have length 1")
def test_normalize_vector(self):
'''Test vector normalization'''
for k in range(10):
rand_vec = np.random.random(k) # Make a random k-length vector
# Ignore the astronomically unlikely case that a vector has near 0 norm
if not np.isclose(np.sum(rand_vec), 0):
normalized = normalize(rand_vec)
norm = np.linalg.norm(normalized)
# Test that the norm is 1
np.testing.assert_almost_equal(
norm,
1.0,
err_msg="The normalized vector did not have length 1")
def d_spiral_point(self, point, radius, granularity=8, revolutions=1, direction='ccw', theta_offset=0, meters_per_rev=0):
"""
Sprials a point using discrete moves
This produces a generator
"""
point = np.array(point)
if direction == 'ccw':
angle_incrment = 2 * np.pi / granularity
else:
angle_incrment = -2 * np.pi / granularity
sprinkles = transformations.euler_matrix(0, 0, angle_incrment)[:3, :3]
# Find first point to go to using boat rotation
next_point = np.append(normalize(self.nav.pose[0][:2] - point[:2]), 0) # Doing this in 2d
radius_increment = meters_per_rev / granularity
for i in range(granularity * revolutions + 1):
new = point + radius * next_point
radius += radius_increment
yield self.set_position(new).look_at(point).yaw_left(theta_offset)
next_point = sprinkles.dot(next_point)
yield self.set_position(new).look_at(point).yaw_left(theta_offset)
def circle_point(self, point, radius, granularity=8, theta_offset=0):
'''
Circle a point whilst looking at it
This produces a generator, so for use:
circle = navigator.move.circle_point([1,2,0], 5)
for p in circle:
yield p.go()
'''
point = np.array(point)
angle_incrment = 2 * np.pi / granularity
sprinkles = transformations.euler_matrix(0, 0, angle_incrment)[:3, :3]
# Find first point to go to using boat rotation
next_point = np.append(normalize(self.nav.pose[0][:2] - point[:2]), 0) # Doing this in 2d
for i in range(granularity + 1):
new = point + radius * next_point
yield self.set_position(new).look_at(point).yaw_left(theta_offset) # Remember this is a generator - not a twisted yield
next_point = sprinkles.dot(next_point)
yield self.set_position(new).look_at(point).yaw_left(theta_offset)
