def arclen_ori(A, B, r, ori):
if ori > 0:
ret = normalize(B - A, 0, 360) * (2 * np.pi * r / 360.0)
elif ori < 0:
ret = normalize(B - A, -360, 0) * (2 * np.pi * r / 360.0)
else:
raise Exception("ori must be -1 or 1")
return abs(ret)
def arclen_ori(A, B, r, ori):
if ori > 0:
ret = normalize(B-A, 0, 360)*(2*np.pi*r/360.0)
elif ori < 0:
ret = normalize(B-A, -360, 0)*(2*np.pi*r/360.0)
else:
raise Exception("ori must be -1 or 1")
return abs(ret)
def circle_right_direction(center, radius, sense, point, angle):
"""
:param center: Circle center, np array
:param sense: Clockwise=-1, Counterclockwise=1
:param point: Point in circunference, np array
:param angle: Degrees, direction starting from 'point'
:return: Bool
"""
A = angle_to(center, point)
right = move_by_radius(center, radius, A + sense)
wrong = move_by_radius(center, radius, A - sense)
right_dist = abs(normalize(angle - angle_to(point, right)))
wrong_dist = abs(normalize(angle - angle_to(point, wrong)))
return right_dist < wrong_dist
def circle_right_direction(center, radius, sense, point, angle):
"""
:param center: Circle center, np array
:param sense: Clockwise=-1, Counterclockwise=1
:param point: Point in circunference, np array
:param angle: Degrees, direction starting from 'point'
:return: Bool
"""
A = angle_to(center, point)
right = move_by_radius(center, radius, A+sense)
wrong = move_by_radius(center, radius, A-sense)
right_dist = abs(normalize(angle-angle_to(point, right)))
wrong_dist = abs(normalize(angle-angle_to(point, wrong)))
return right_dist < wrong_dist
def arctan2(co, ca):
"""
:param co: Number, cateto opuesto
:param ca: Number, cateto adyacente
:return: Degrees
"""
ang = np.arctan2(co, ca)
return normalize(np.degrees(ang))
def arctan2(co, ca):
"""
:param co: Number, cateto opuesto
:param ca: Number, cateto adyacente
:return: Degrees
"""
ang = np.arctan2(co, ca)
return normalize(np.degrees(ang))
def act(self):
while not rospy.is_shutdown():
self._rate.sleep()
if self._human_joints is None:
rospy.logwarn("No human joints received, waiting...")
continue
#left arm
pr2_joints_left = [0] * 7
pr2_joints_left[0] = normalize(self._human_joints[2], -pi, pi)
pr2_joints_left[1] = normalize(-(self._human_joints[4] - pi/2), -pi, pi)
pr2_joints_left[2] = normalize(-(self._human_joints[6] - pi/2), -pi, pi)
pr2_joints_left[3] = normalize(self._human_joints[0], -pi, pi)
#right_arm
pr2_joints_right = [0] * 7
pr2_joints_right[0] = normalize(self._human_joints[3], -pi, pi)
pr2_joints_right[1] = normalize(-(self._human_joints[5] - pi/2), -pi, pi)
pr2_joints_right[2] = normalize((self._human_joints[7] - pi/2), -pi, pi)
pr2_joints_right[3] = normalize(self._human_joints[1], -pi, pi)
#get left fk pose
l_execute = True
l_pose = self._ik.get_left_arm_fk_pose(pr2_joints_left,
self._pr2box.frame_id)[0]
rospy.loginfo("testing pose: %s", l_pose)
if (l_pose[1] < 0.2):
rospy.logwarn("Left arm too close to center, not executing"
)
l_execute = False
if self._pr2box.point_inside(l_pose):
rospy.logwarn("Warning, desired pose: %s is dangerous for left arm,"
"not executing!", l_pose)
l_execute = False
if (l_execute):
self._pr2.set_arm_state(pr2_joints_left, "left", wait=False)
#get right ik pose
r_pose = self._ik.get_right_arm_fk_pose(pr2_joints_right,
self._pr2box.frame_id)[0]
r_execute = True
if (r_pose[1] > -0.2):
rospy.logwarn("Right arm too close to center, not executing"
)
r_execute = False
if (self._pr2box.point_inside(r_pose)):
rospy.logwarn("Warning, desired pose: %s is dangerous for right arm,"
"not executing!", r_pose)
r_execute = False
if (r_execute):
self._pr2.set_arm_state(pr2_joints_right, "right", wait=False)
def test_normalize_b_false_outside_left_limit():
"""Values to the left of the left limit must be handled properly."""
L, U = 0, 360
assert normalize(L-1, L, U) == U - 1
assert normalize(-360, L, U) == 0
assert normalize(-361, L, U) == 359
assert normalize(-721, L, U) == 359
L, U = -90, 90
assert normalize(L-1, L, U) == U - 1
assert normalize(-180, L, U) == 0
assert normalize(-181, L, U) == -1
assert normalize(-361, L, U) == -1
def test_normalize_b_false_outside_left_limit():
"""Values to the left of the left limit must be handled properly."""
L, U = 0, 360
assert normalize(L - 1, L, U) == U - 1
assert normalize(-360, L, U) == 0
assert normalize(-361, L, U) == 359
assert normalize(-721, L, U) == 359
L, U = -90, 90
assert normalize(L - 1, L, U) == U - 1
assert normalize(-180, L, U) == 0
assert normalize(-181, L, U) == -1
assert normalize(-361, L, U) == -1
def test_normalize_proper_input_range():
"""Normalize must be called with proper range limits must be"""
with pytest.raises(ValueError):
normalize(10, 0, 360, b=True)
with pytest.raises(ValueError):
normalize(10, -89, 90, b=True)
with pytest.raises(ValueError):
normalize(10, 1, 90)
# both shouldn't raise any exception
assert normalize(10, 0, 360) == 10
assert normalize(10, -90, 90) == 10
def test_normalize_proper_input_range():
"""Normalize must be called with proper range limits must be"""
with pytest.raises(ValueError):
normalize(10, 0, 360, b=True)
with pytest.raises(ValueError):
normalize(10, -89, 90, b=True)
with pytest.raises(ValueError):
normalize(10, 1, 90)
# both shouldn't raise any exception
assert normalize(10, 0, 360) == 10
assert normalize(10, -90, 90) == 10
def set_control(self, vel, steer):
norm_steer = angles.normalize(steer, -2*pi, 2*pi)
self.control_vel = vel
if self.control_vel > self.max_vel:
self.control_vel = self.max_vel
if self.control_vel > self.max_vel:
self.control_vel = self.min_vel
self.control_angle = norm_steer
if self.control_angle > self.max_steer:
self.control_angle = self.max_steer
if self.control_angle > self.max_steer:
self.control_angle = self.min_steer
def mgs84_norm_lat(angle):
"""
Noralize latitude to the MGS84 datum. For storing data where only MGS84 is supported
(e.g. MongoDB)
Parameters
----------
angle : double
The angle to normalize
Returns
-------
: double
The new Angle normalized between [-90, 90]
"""
return normalize(angle, lower=-90, upper=90)
def test_normalize_b_true_sanity_check():
"""Values at extremes and in the middle must be handled."""
L, U = -180, 180
assert normalize(10, L, U, b=True) == 10
assert normalize(L, L, U, b=True) == L
assert normalize(U, L, U, b=True) == U
L, U = -90, 90
assert normalize(10, L, U, b=True) == 10
assert normalize(L, L, U, b=True) == L
assert normalize(U, L, U, b=True) == U
def test_normalize_b_false_outside_right_limit():
"""Values to the right of the right limit must be handled properly."""
L, U = 0, 360
assert normalize(U+1, L, U) == L + 1
assert normalize(U*2+1, L, U) == L + 1
assert normalize(U*100+(U-L)/2.0, L, U) == L + (U-L)/2.0
L, U = -90, 90
assert normalize(U+1, L, U) == L + 1
assert normalize(181, L, U) == 1
assert normalize(361, L, U) == 1
def test_normalize_b_true_outside_left_limit():
"""Values to the left of the left limit must be handled properly."""
L, U = -180, 180
assert normalize(-181, L, U, b=True) == -179
assert normalize(-361, L, U, b=True) == 1
assert normalize(-721, L, U, b=True) == -1
L, U = -90, 90
assert normalize(-91, L, U, b=True) == -89
assert normalize(-181, L, U, b=True) == 1
assert normalize(-361, L, U, b=True) == -1
def test_normalize_b_true_outside_right_limit():
"""Values to the right of the right limit must be handled properly."""
L, U = -180, 180
assert normalize(U+1, L, U, b=True) == U - 1
assert normalize(181, L, U, b=True) == U - 1
assert normalize(361, L, U, b=True) == -1
L, U = -90, 90
assert normalize(U+1, L, U, b=True) == U - 1
assert normalize(181, L, U, b=True) == -1
assert normalize(361, L, U, b=True) == 1
def test_normalize_b_false_outside_right_limit():
"""Values to the right of the right limit must be handled properly."""
L, U = 0, 360
assert normalize(U + 1, L, U) == L + 1
assert normalize(U * 2 + 1, L, U) == L + 1
assert normalize(U * 100 + (U - L) / 2.0, L, U) == L + (U - L) / 2.0
L, U = -90, 90
assert normalize(U + 1, L, U) == L + 1
assert normalize(181, L, U) == 1
assert normalize(361, L, U) == 1
def test_normalize_b_true_sanity_check():
"""Values at extremes and in the middle must be handled."""
L, U = -180, 180
assert normalize(10, L, U, b=True) == 10
assert normalize(L, L, U, b=True) == L
assert normalize(U, L, U, b=True) == U
L, U = -90, 90
assert normalize(10, L, U, b=True) == 10
assert normalize(L, L, U, b=True) == L
assert normalize(U, L, U, b=True) == U
def test_normalize_b_false_sanity_check():
"""Values at extremes and in the middle must be handled."""
L, U = 0, 360
assert normalize(10, L, U) == 10
assert normalize(L, L, U) == L
assert normalize(U, L, U) == L
L, U = -90, 90
assert normalize(10, L, U) == 10
assert normalize(L, L, U) == L
assert normalize(U, L, U) == L
def test_normalize_b_true_outside_right_limit():
"""Values to the right of the right limit must be handled properly."""
L, U = -180, 180
assert normalize(U + 1, L, U, b=True) == U - 1
assert normalize(181, L, U, b=True) == U - 1
assert normalize(361, L, U, b=True) == -1
L, U = -90, 90
assert normalize(U + 1, L, U, b=True) == U - 1
assert normalize(181, L, U, b=True) == -1
assert normalize(361, L, U, b=True) == 1
def test_normalize_b_true_outside_left_limit():
"""Values to the left of the left limit must be handled properly."""
L, U = -180, 180
assert normalize(-181, L, U, b=True) == -179
assert normalize(-361, L, U, b=True) == 1
assert normalize(-721, L, U, b=True) == -1
L, U = -90, 90
assert normalize(-91, L, U, b=True) == -89
assert normalize(-181, L, U, b=True) == 1
assert normalize(-361, L, U, b=True) == -1
def test_normalize_b_false_sanity_check():
"""Values at extremes and in the middle must be handled."""
L, U = 0, 360
assert normalize(10, L, U) == 10
assert normalize(L, L, U) == L
assert normalize(U, L, U) == L
L, U = -90, 90
assert normalize(10, L, U) == 10
assert normalize(L, L, U) == L
assert normalize(U, L, U) == L
def process(self):
self.output.twist.linear.x = self.pid.x.update(
self.input_.pose.x, self.state.position[0], self.state.twist.linear[0], self.dt
)
self.output.twist.linear.y = self.pid.y.update(
self.input_.pose.y, self.state.position[1], self.state.twist.linear[1], self.dt
)
self.output.twist.linear.z = self.pid.z.update(
self.input_.pose.z, self.state.position[2], self.state.twist.linear[2], self.dt
)
yaw_command = angles.normalize(
self.input_.pose.yaw, self.state.euler[2] - math.pi, self.state.euler[2] + math.pi
)
self.output.twist.angular.z = self.pid.yaw.update(
yaw_command, self.state.euler[2], self.state.twist.angular[2], self.dt
)
if self.verbose:
utils.pv("self.state.euler[2]", "yaw_command")
def process(self):
self.output.twist.linear.x = self.pid.x.update(
self.input_.pose.x, self.state.position[0],
self.state.twist.linear[0], self.dt)
self.output.twist.linear.y = self.pid.y.update(
self.input_.pose.y, self.state.position[1],
self.state.twist.linear[1], self.dt)
self.output.twist.linear.z = self.pid.z.update(
self.input_.pose.z, self.state.position[2],
self.state.twist.linear[2], self.dt)
yaw_command = angles.normalize(self.input_.pose.yaw,
self.state.euler[2] - math.pi,
self.state.euler[2] + math.pi)
self.output.twist.angular.z = self.pid.yaw.update(
yaw_command, self.state.euler[2], self.state.twist.angular[2],
self.dt)
if self.verbose:
utils.pv('self.state.euler[2]', 'yaw_command')
def _step_cog(self):
"""Calculate the course between two position points"""
def course_utm(df):
df.reset_index(inplace=True)
df["Step_Azimuth"] = azimuth_utm(
df["geometry"].shift(), df.loc[1:, "geometry"]
)
return df.set_index("index")
# Calculate and normalize course between successive points
self.gdf = self.grouped_trip.apply(course_utm)
self.gdf["Step_Azimuth"].fillna(method="bfill", inplace=True)
self.gdf["Step_Azimuth"] = round(
self.gdf["Step_Azimuth"].apply(lambda x: angles.normalize(x, 0, 360))
)
# Caclulate error
self.gdf["Error_COG"] = 180 - abs(
abs(self.gdf["COG"] - self.gdf["Step_Azimuth"]) - 180
)
self.gdf["Error_Heading"] = 180 - abs(
abs(self.gdf["Heading"] - self.gdf["Step_Azimuth"]) - 180
)
def angle_to(a, b):
v = vector_to(a, b)
return normalize(arctan2(v[1], v[0]))
def min_angular_distance(A, B):
return abs(normalize(A - B))
def arclen(A, B, r):
return normalize(B - A, 0, 360) * (2 * np.pi * r / 360.0)
def normalizeRad(self, value): deg = angles.r2d(float(value)) return angles.d2r(angles.normalize(deg, -180, 180))
def arccos(x):
return normalize(np.degrees(np.arccos(x)))
def _normalize_angles(self):
"""Normalize COG to an angle between [0, 360)."""
self.df["COG"] = self.df["COG"].apply(lambda x: angles.normalize(x, 0, 360))
self.df["Heading"] = self.df["Heading"].apply(
lambda x: angles.normalize(x, 0, 360)
)
def step_autonomy(self, t, dt):
super(LocalFlocking, self).step_autonomy(t, dt)
# If outside of battlebox, head towards own base!"
if not self._battlebox.contains(self._own_pose.pose.pose.position.lat,
self._own_pose.pose.pose.position.lon,
self._own_pose.pose.pose.position.alt):
self._wp = np.array(
[self._own_base[0], self._own_base[1], self._last_ap_wp[2]])
return True
# Compute distance to each blue uav, tuple: (id, distance)
# Don't include ourself
blue_dists = [
(id, np.linalg.norm(self.state.pos[:2] - state.pos[:2]))
for id, state in self.local_blues.iteritems() if id != self._id
and self.state.in_field_of_view(state.pos, math.pi, math.pi)
]
# Experimental: only use neighbors in front of own uav
#
# Sort blue distances
blue_dists.sort(key=lambda x: x[1])
# return nearest neighbors
neighbors = blue_dists[:self._max_neighbors]
# Separation: steer to avoid crowding local flockmates
sep_vec_sum = np.array([0, 0])
# Alignment: steer towards the average heading of local flockmates
yaw_sum = 0
# Cohesion: steer to move toward the average position (centroid) of
# local flockmates
centroid_sum = np.array([0, 0])
# Set gains for each behavior
sep_gain = 0.05
yaw_gain = 1
coh_gain = 0.05
tgt_gain = self._target_gain
box_gain = 10
# calculate running sum for each neighbor-based behavior
for id, state in neighbors:
# Calculate vector pointing from neighbor to own position
vec_diff = (self.state.pos[:2] - self.local_blues[id].pos[:2])
# Running sum of vectors pointing from neighbor to own position for
# separation vector calculation
sep_vec_sum = sep_vec_sum + vec_diff / np.linalg.norm(vec_diff)
# Running sum of neighbor headings for heading alignment
# calculation
yaw_sum = yaw_sum + self.local_blues[id].yaw()
# Running sum of neighbor positions for centroid calculation
centroid_sum = centroid_sum + self.local_blues[id].pos[:2]
# Initialize vectors to zero
vec_result = np.array([0, 0])
sep_vec = np.array([0, 0])
yaw_vec = np.array([0, 0])
coh_vec = np.array([0, 0])
tgt_vec = np.array([0, 0])
box_vec = np.array([0, 0])
# Normalize / average neighbor-based behaviors
if len(neighbors) > 0:
sep_vec = sep_vec_sum / len(neighbors) # Normalize separation vec
avg_yaw = yaw_sum / len(neighbors) # Average yaw calc
centroid = centroid_sum / len(neighbors) # Centroid of neighbors
# Convert desired centroid to velocity pointing from own position
# to centroid
coh_vec = centroid - self.state.pos[:2]
# Convert average yaw to vector pointing in direction for own uav
# to achieve average yaw
yaw_vec = np.array([math.cos(avg_yaw), math.sin(avg_yaw)])
# Compute distance to each red uav, tuple: (id, distance)
red_dists = [ (id, np.linalg.norm(self.state.pos[:2]-state.pos[:2])) \
for id, state in self.local_reds.iteritems() ]
# Determine nearest enemy
try:
closest_enemy_id = min(red_dists, key=lambda t: t[1])[0]
tgt_dir = self.local_reds[
closest_enemy_id].pos[:2] - self.state.pos[:2]
tgt_vec = tgt_dir / np.linalg.norm(tgt_dir)
except ValueError:
# No enemies exist, zero out target vector
tgt_vec = np.array([0, 0])
# Get time-to-breach closest battle cube wall.
wall_time, wall_angle = self.closest_wall_time_angle()
# If the time-to-breach closest wall is less than 10 seconds, calculate
# avoidance velocity vector
if wall_time < self._min_wall_time:
box_yaw = 0
if wall_angle > 0:
# Turn hard left (increase angle) if the wall_angle is greater
# than 0
box_yaw = normalize(self.state.yaw() + math.pi - 0.1, -math.pi,
math.pi)
else:
# Turn hard right (decrease angle) if the wall_angle is greater
# than 0
box_yaw = normalize(self.state.yaw() - math.pi - 0.1, -math.pi,
math.pi)
# Convert desired yaw to velocity vector
box_vec = np.array([math.cos(box_yaw), math.sin(box_yaw)])
# Perform linear combination of each flocking behavior
vec_result = sep_gain * sep_vec + \
yaw_gain * yaw_vec + \
coh_gain * coh_vec + \
tgt_gain * tgt_vec + \
box_gain * box_vec
# If the vector sum of all flocking behaviors is very small, set the
# velocity to the current velocity
if np.linalg.norm(vec_result) < 0.001:
vec_result = self.state.vel[:2]
# Convert local desired velocity vector to a GPS waypoint
self._wp = self.vector_2_waypoint(vec_result, alt_deconflict=True)
# Determine if any enemy aircraft can be tagged
for uav_id in self.local_reds:
if self.hitable(uav_id):
self._action = ["Fire", uav_id]
break
return True
def _move_on_torsion(self, residue_number, move_pose, get_angle,
set_angle):
new_torsion_position = normalize(
get_angle(residue_number) + move_pose, -180, 180)
set_angle(residue_number, new_torsion_position)
def norm_angle(x):
return angles.normalize(x, 0, 2*pi)
def min_angular_distance(A, B):
return abs(normalize(A-B))
def angle_to(a, b):
v = vector_to(a, b)
return normalize(arctan2(v[1], v[0]))
def arccos(x):
return normalize(np.degrees(np.arccos(x)))
def arclen(A, B, r):
return normalize(B-A, 0, 360)*(2*np.pi*r/360.0)
def normalizeAngles(angleList, angle_range):
return np.array([angles.normalize(i, angle_range[0], angle_range[1])
for i in angleList])
def normalize_angles(cls, radian_vector):
return np.asarray(map(lambda x: angles.d2r(angles.normalize(angles.r2d(x), -180, +180)), radian_vector))
