def _s_turn(self):
''' Calculations associated with the S-turn to the virtual target
'''
if self._s_turn_state == self.S1: # Offset turn
self._gain = 2.0
self._bias = 0.5 * gps.signum(self._theta_dot)
if math.fabs(gps.signum(self._theta_dot) - \
gps.signum(self._theta_dot_last)) == 2:
self._s_turn_state = self.S2
self._theta2 = self._theta
self._theta3 = self._theta2 + 0.75 * (self._theta_VT_init -
self._theta2)
self._theta3 = gps.normalize(self._theta3)
self._s2_check = \
gps.signum(gps.normalize_pi(self._theta - self._theta3))
self.log_info("Shift to S-turn phase S2:\tTheta=%f\tTheta3=%f\tTheta_dot=%f"\
%(self._theta, self._theta3, self._theta_dot))
if self._s_turn_state == self.S2: # Middle portion of the S
self._bias = -0.25 * gps.signum(self._theta_dot)
self._gain = -2.0
if self._s2_check != \
gps.signum(gps.normalize_pi(self._theta - self._theta3)):
self._s_turn_state = self.S3
self._theta3 = self._theta
self._psi_p3 = self._xp[2]
self.log_info("Shift to S-turn phase S3")
self._bias_initialized = False # to facilitate a log entry
if self._s_turn_state == self.S3: # Final turn to VT
if (self._theta_dot_VT_init < 0.0 and \
angle_between(self._psi_p_f, self._theta_VT_init, \
2.0 * self._theta3 - self._psi_p3)) or \
(self._theta_dot_VT_init >= 0.0 and \
angle_between(self._psi_p_f, \
2.0 * self._theta3 - self._psi_p3, \
self._theta_VT_init)):
self._gain = math.fabs((gps.normalize_pi(self._psi_p_f - self._psi_p3)) / \
(gps.normalize_pi(self._psi_p_f - self._theta3)))
self._bias = 0.0
if not self._bias_initialized:
self._bias_initialized = True
self.log_info("Phase S3 using 1-bias turn")
else:
if math.fabs(gps.normalize_pi(self._psi_p_f - self._xp[2]) / \
gps.normalize_pi(self._psi_p_f - self._theta)) < 2.0:
self._gain = gps.normalize_pi(self._psi_p3) / \
gps.normalize_pi(math.pi/2.0 + self._theta3)
self._bias = 0.0
else:
self._gain = 2.0
self._bias = 0.0
if not self._bias_initialized:
self._bias_initialized = True
self.log_info("Phase S3 using 2-bias turn")
def _parallel_2_state(self):
''' Performs calculations and operations required for the "parallel 2" state
'''
if not self._state_initialized:
self._psi_p_f = gps.normalize(self._xt[2] -
USMAInterceptor.FIRE_ANGLE)
self._bias = 0.0
self._state_initialized = True
self._set_ctrl_params(self._xp, self._vp, self._xt, self._vt)
self._gain = math.fabs(gps.normalize_pi(self._psi_p_f - self._xp[2]) / \
gps.normalize_pi(self._psi_p_f - self._theta))
self._E = self._calculate_E()
if self._E > min(numpy.mean(self._E_trend), \
numpy.median(self._E_trend)): # reached minimum
self._state_initialized = False
self._state = USMAInterceptor.INTERCEPT
self.log_info("Transition to intercept phase")
self._E_trend = self._E_trend[1:] + [self._E]
def compute_intercept_waypoint(self, wpt=None):
''' Computes an intercept waypoint for the UAV to steer to
@param wpt: waypoint object to use for computed values
@return waypoint object (LLA) with the intercept waypoint (or None)
'''
lead_uav = self._pre_check()
if not lead_uav:
return None
# Update pursuer and target position and velocity info
lead_lat, lead_lon = \
gps.gps_newpos(lead_uav.state.pose.pose.position.lat, \
lead_uav.state.pose.pose.position.lon, \
self._follow_distance, self._follow_angle)
lead_spd = math.hypot(lead_uav.state.twist.twist.linear.x, \
lead_uav.state.twist.twist.linear.y)
lead_crs = math.atan2(lead_uav.state.twist.twist.linear.y, \
lead_uav.state.twist.twist.linear.x)
own_lat = self.own_pose.pose.pose.position.lat
own_lon = self.own_pose.pose.pose.position.lon
own_spd = math.hypot(self.own_pose.twist.twist.linear.x, \
self.own_pose.twist.twist.linear.y)
if (own_spd - lead_spd) < 1.0: own_spd = lead_spd + 1.0
own_crs = math.atan2(self.own_pose.twist.twist.linear.y, \
self.own_pose.twist.twist.linear.x)
# Compute the control parameters
self._theta = gps.gps_bearing(own_lat, own_lon, lead_lat, lead_lon)
tgt_distance = gps.gps_distance(own_lat, own_lon, lead_lat, lead_lon)
crossing_spd = own_spd * math.sin(self._theta - own_crs) + \
lead_spd * math.sin(self._theta + math.pi - lead_crs)
self._theta_dot = crossing_spd / tgt_distance
theta_diff = gps.normalize_pi(self._theta - own_crs)
self._bias = min(gps.signum(theta_diff) * 2.5, theta_diff / 10.0)
# Use the PN calculation to compute the ordered acceleration
ordered_a = (self._gain * self._theta_dot + self._bias) * own_spd
a_max = min(self._a_max, ((2.0 * math.pow(own_spd, 2.0)) / self._L1))
if math.fabs(ordered_a) > a_max:
ordered_a = gps.signum(ordered_a) * a_max
nu = math.asin((ordered_a * self._L1) / (2.0 * math.pow(own_spd, 2.0)))
# Compute the waypoint command
if not wpt: wpt = brdgmsg.LLA()
wpt.lat, wpt.lon = \
gps.gps_newpos(lead_lat, lead_lon, nu + own_crs, self._L1)
if self._alt_mode == InterceptCalculator.BASE_ALT_MODE:
wpt.alt = self._alt
else:
wpt.alt = lead_uav.state.pose.pose.rel_alt + self._alt
self._owner.log_dbug(
"intercept waypoint (lat, lon, alt) = (%f, %f, %f)" %
(wpt.lat, wpt.lon, wpt.alt))
return wpt
def compute_intercept_waypoint(self, wpt=None):
''' Computes an intercept waypoint for the UAV to steer to
@param wpt: waypoint object to use for computed values
@return waypoint object (LLA) with the intercept waypoint (or None)
'''
lead_uav = self._pre_check()
if not lead_uav:
return None
# Update pursuer and target position and velocity info
lead_lat, lead_lon = \
gps.gps_newpos(lead_uav.state.pose.pose.position.lat, \
lead_uav.state.pose.pose.position.lon, \
self._follow_distance, self._follow_angle)
lead_spd = math.hypot(lead_uav.state.twist.twist.linear.x, \
lead_uav.state.twist.twist.linear.y)
lead_crs = math.atan2(lead_uav.state.twist.twist.linear.y, \
lead_uav.state.twist.twist.linear.x)
own_lat = self.own_pose.pose.pose.position.lat
own_lon = self.own_pose.pose.pose.position.lon
own_spd = math.hypot(self.own_pose.twist.twist.linear.x, \
self.own_pose.twist.twist.linear.y)
if (own_spd - lead_spd) < 1.0: own_spd = lead_spd + 1.0
own_crs = math.atan2(self.own_pose.twist.twist.linear.y, \
self.own_pose.twist.twist.linear.x)
# Compute the control parameters
self._theta = gps.gps_bearing(own_lat, own_lon, lead_lat, lead_lon)
tgt_distance = gps.gps_distance(own_lat, own_lon, lead_lat, lead_lon)
crossing_spd = own_spd * math.sin(self._theta - own_crs) + \
lead_spd * math.sin(self._theta + math.pi - lead_crs)
self._theta_dot = crossing_spd / tgt_distance
theta_diff = gps.normalize_pi(self._theta - own_crs)
self._bias = min(gps.signum(theta_diff) * 2.5, theta_diff / 10.0)
# Use the PN calculation to compute the ordered acceleration
ordered_a = (self._gain * self._theta_dot + self._bias) * own_spd
a_max = min(self._a_max, ((2.0 * math.pow(own_spd, 2.0)) / self._L1))
if math.fabs(ordered_a) > a_max:
ordered_a = gps.signum(ordered_a) * a_max
nu = math.asin((ordered_a * self._L1) / (2.0 * math.pow(own_spd, 2.0)))
# Compute the waypoint command
if not wpt: wpt = brdgmsg.LLA()
wpt.lat, wpt.lon = \
gps.gps_newpos(lead_lat, lead_lon, nu + own_crs, self._L1)
if self._alt_mode == InterceptCalculator.BASE_ALT_MODE:
wpt.alt = self._alt
else:
wpt.alt = lead_uav.state.pose.pose.rel_alt + self._alt
self._owner.log_dbug("intercept waypoint (lat, lon, alt) = (%f, %f, %f)"
%(wpt.lat, wpt.lon, wpt.alt))
return wpt
def _runin_state(self):
''' Performs calculations and operations required for the "runin" state
'''
if not self._state_initialized:
self.log_info("Commence run-in phase")
self._gain = 2.5
self._state_initialized = True
# Compute the control parameters & set a bias value if rqd
# to force the pursuer towards the target
self._set_ctrl_params(self._xp, self._vp, self._xt, self._vt)
theta_diff = gps.normalize_pi(self._theta - self._xp[2])
self._bias = min(gps.signum(theta_diff) * 2.5, theta_diff / 10.0)
# Keep going until the offset range is reached
if self._range <= PNInterceptor.R_TH1:
self._state_initialized = False
self._state = PNInterceptor.OFFSET
self.log_info("Transition to offset phase")
def _runin_state(self):
''' Performs calculations and operations required for the "runin" state
'''
if not self._state_initialized:
self.log_info("Commence run-in phase")
self._gain = 2.5
self._state_initialized = True
# Compute the control parameters & set a bias value if rqd
# to force the pursuer towards the target
self._set_ctrl_params(self._xp, self._vp, self._xt, self._vt)
theta_diff = gps.normalize_pi(self._theta - self._xp[2])
self._bias = min(gps.signum(theta_diff) * 2.5, theta_diff / 10.0)
# Keep going until the offset range is reached
if self._range <= PNInterceptor.R_TH1:
self._state_initialized = False
self._state = PNInterceptor.OFFSET
self.log_info("Transition to offset phase")
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
# Search for the closest target every frame
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
# Calculate target and own position and orientation
target_pos = self._reds[uav.vehicle_id].state.pose.pose.position
target_orientation = self._reds[uav.vehicle_id].state.pose.pose.orientation
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
if self._bboxes[0].contains(tgt_lat, tgt_lon, tgt_alt): # added to only consider those in battlebox
own_pos = self._own_pose.pose.pose.position
own_lat = own_pos.lat
own_lon = own_pos.lon
own_alt = own_pos.rel_alt
own_orientation = self._own_pose.pose.pose.orientation
# Calculate bearing to target and from target to self
bearing = gps.normalize(gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon))
tgt_bearing = gps.normalize(gps.gps_bearing(tgt_lat, tgt_lon, own_lat, own_lon))
# Calculate heading/yaw of self and target
own_rpy = qmath.quat_to_euler((own_orientation.x,\
own_orientation.y, \
own_orientation.z, \
own_orientation.w))
own_heading = gps.normalize(own_rpy[2])
tgt_rpy = qmath.quat_to_euler((target_orientation.x,\
target_orientation.y, \
target_orientation.z, \
target_orientation.w))
tgt_heading = gps.normalize(tgt_rpy[2])
# Calculate distance to target
dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon)
bearing_adj = 0.0
# Calculate offset between bearing and target heading (measures degree of "head on")
# ----------------------------------------------------------------------------
heading_diff = own_heading - tgt_heading
tgt_bearing_diff = gps.normalize_pi( tgt_heading - tgt_bearing )
if math.fabs(heading_diff) > math.pi/2:
# moving towards each other
if tgt_bearing > (tgt_heading - self._fov_width_radians/2) or tgt_bearing < (tgt_heading + self._fov_width_radians/2):
# we are in the "fan"
if dist > 2*self._max_range and math.fabs(tgt_bearing_diff) < self._fov_width_radians/2:
bearing_adj = math.copysign(ADJ_ANGLE, tgt_bearing_diff)
#print 'ID: {} toward UAV: {}: adjusting bearing by {}'.format(self._id, uav.vehicle_id, math.degrees(bearing_adj))
elif dist < self._max_range and math.fabs(tgt_bearing_diff) > self._fov_width_radians*3:
# along side each other
bearing_adj = math.copysign(3*ADJ_ANGLE, tgt_bearing_diff)
#print 'ID: {} along side UAV: {}: adjusting bearing by {}'.format(self._id, uav.vehicle_id, math.degrees(bearing_adj))
else:
# heading in same general direction
if math.fabs(tgt_bearing_diff) < math.pi/2:
# I am in front of target
if dist < 2*self._max_range and math.fabs(tgt_bearing_diff) < self._fov_width_radians/2:
bearing_adj = -math.copysign(ADJ_ANGLE, tgt_bearing_diff)
#print 'ID: {} away from UAV: {} adjusting bearing by {}'.format(self._id, uav.vehicle_id, math.degrees(bearing_adj))
# ----------------------------------------------------------------------------
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing + bearing_adj, 1000)
self._wp = np.array([lat, lon, own_alt]) # keep own alt
# Determine if the target is within firing parameters
if gps.hitable(own_lat, own_lon, own_alt, \
(own_orientation.x, \
own_orientation.y, \
own_orientation.z, \
own_orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
tgt_lat, tgt_lon, tgt_alt):
self._action = ["Fire", uav.vehicle_id]
print "EA=======================>ID: %d shot at Target %d" % (self._id, uav.vehicle_id)
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
self._target_id = None
# Search for the closest target every frame
min_dist = np.inf
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon, \
uav.state.pose.pose.position.rel_alt):
self._action = ["Fire", uav.vehicle_id]
print "RT=========(loop)========>ID: %d shot at Target %d" % (
self._id, uav.vehicle_id)
'''if self._last_target_id == self._target_id:
if d < 100: # how close do we have to be before I call chasing
self._target_count += 1
if self._target_count > 100:
self._target_count = 300
self._target_id = None
elif self._target_count > 0:
self._target_count -= 1
self._target_id = None
'''
# If a target has been identified, move towards it
if self._target_id != None:
self._last_target_id = self._target_id
# Calculate target and own position and orientation
target_pos = self._reds[self._target_id].state.pose.pose.position
target_orientation = self._reds[
self._target_id].state.pose.pose.orientation
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
own_pos = self._own_pose.pose.pose.position
own_lat = own_pos.lat
own_lon = own_pos.lon
own_alt = own_pos.rel_alt
own_orientation = self._own_pose.pose.pose.orientation
# Calculate bearing to target and from target to self
bearing = gps.normalize(
gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon))
tgt_bearing = gps.normalize(
gps.gps_bearing(tgt_lat, tgt_lon, own_lat, own_lon))
# Calculate heading/yaw of self and target
own_rpy = qmath.quat_to_euler((own_orientation.x,\
own_orientation.y, \
own_orientation.z, \
own_orientation.w))
own_heading = gps.normalize(own_rpy[2])
tgt_rpy = qmath.quat_to_euler((target_orientation.x,\
target_orientation.y, \
target_orientation.z, \
target_orientation.w))
tgt_heading = gps.normalize(tgt_rpy[2])
# Calculate distance to target
dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon)
bearing_adj = 0.0
#print 'ID: {} targeting UAV: {} at bearing {} and distance {}'.format(self._id, self._target_id, math.degrees(bearing), dist)
#print 'UAV: {} own bearing: {}'.format(self._target_id, math.degrees(tgt_heading))
# Calculate offset between bearing and target heading (measures degree of "head on")
# This section needs more work
# ----------------------------------------------------------------------------
heading_diff = own_heading - tgt_heading
tgt_bearing_diff = gps.normalize_pi(tgt_heading - tgt_bearing)
if math.fabs(heading_diff) > math.pi / 2:
# moving towards each other
if tgt_bearing > (
tgt_heading - self._fov_width_radians / 2
) or tgt_bearing < (tgt_heading + self._fov_width_radians / 2):
# we are in the "fan"
if dist > self._max_range and math.fabs(
tgt_bearing_diff) < self._fov_width_radians / 2:
bearing_adj = math.copysign(ADJ_ANGLE,
tgt_bearing_diff)
print 'ID: {} toward UAV: {}: adjusting bearing by {}'.format(
self._id, self._target_id,
math.degrees(bearing_adj))
else:
# heading in same general direction
if math.fabs(tgt_bearing_diff) < math.pi / 2:
# I am in front of target
if dist < 2 * self._max_range and math.fabs(
tgt_bearing_diff) < self._fov_width_radians / 2:
bearing_adj = -math.copysign(ADJ_ANGLE,
tgt_bearing_diff)
print 'ID: {} away from UAV: {} adjusting bearing by {}'.format(
self._id, self._target_id,
math.degrees(bearing_adj))
# ----------------------------------------------------------------------------
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing + bearing_adj,
1000)
self._wp = np.array([lat, lon, own_alt]) # keep own alt
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", self._target_id]
print "RT=======================>ID: %d shot at Target %d" % (
self._id, self._target_id)
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
return True
def _offset_state(self):
''' Performs calculations and operations required for the "offset" state
'''
if not self._state_initialized:
# Set the virtual waypoint D2 meters along the target's path and
# offset by D1 meters to the side that the pursuer is currently on
theta = gps.gps_bearing(self._xp[0], self._xp[1], \
self._xt[0], self._xt[1])
offset_dir = \
gps.signum(gps.normalize_pi(theta + math.pi - self._xt[2]))
tgt_to_VT = self._xt[2] + math.atan2(USMAInterceptor.D1 * offset_dir, \
USMAInterceptor.D2)
self._VT = \
gps.gps_newpos(self._xt[0], self._xt[1], tgt_to_VT, \
math.hypot(USMAInterceptor.D1, USMAInterceptor.D2))
self._set_ctrl_params(self._xp, self._vp, \
(self._VT[0], self._VT[1], self._xt[2]), \
(0.0, 0.0))
self._theta_VT_init = self._theta
self._theta_dot_VT_init = self._theta_dot
self._psi_p_VT_init = self._xp[2]
self._psi_p_f = gps.normalize(self._xt[2] + math.pi)
psi_p_f_rel = gps.normalize(self._psi_p_f - self._theta_VT_init)
if gps.normalize_pi(self._theta_VT_init -
self._psi_p_VT_init) > 0.0:
psi_p_f_rel = gps.normalize(-psi_p_f_rel)
if angle_between(psi_p_f_rel, 0.0, math.pi):
self._VT_turn_type = USMAInterceptor.S_TURN
self._s_turn_state = USMAInterceptor.S1
elif angle_between(psi_p_f_rel, (self._theta_VT_init - \
self._psi_p_VT_init), 0.0):
self._VT_turn_type = USMAInterceptor.DIRECT_TURN
else:
self._VT_turn_type = USMAInterceptor.DIRECT_2BIAS
self._state_initialized = True
self._set_ctrl_params(self._xp, self._vp, \
(self._VT[0], self._VT[1], 0.0), (0.0, 0.0))
if self._range < USMAInterceptor.R_CAP_PARALLEL:
self._state_initialized = False
self._state = USMAInterceptor.PARALLEL_1
self.log_info("Transition to parallel_1 phase")
# Determine how to maneuver to the virtual target point
# Direct turn to the virtual target
if self._VT_turn_type == USMAInterceptor.DIRECT_TURN:
self._gain = math.fabs((gps.normalize_pi(self._psi_p_f - \
self._psi_p_VT_init)) / \
(gps.normalize_pi(self._psi_p_f - \
self._theta_VT_init)))
self._bias = 0.0
# Single turn to virtual target with 2 gains
elif self._VT_turn_type == USMAInterceptor.DIRECT_2BIAS:
if math.fabs(gps.normalize_pi(self._psi_p_f - self._xp[2]) / \
gps.normalize_pi(self._psi_p_f - self._theta)) < 2.0:
self._gain = gps.normalize_pi(self._psi_p_VT_init) / \
gps.normalize_pi(math.pi/2 + self._theta_VT_init)
self._bias = 0.0
else:
self._gain = 2.0
self._bias = 0.0
# S-turn to virtual target
else:
self._s_turn()
def _offset_state(self):
''' Performs calculations and operations required for the "offset" state
'''
if not self._state_initialized:
# Set the virtual waypoint D2 meters along the target's path and
# offset by D1 meters to the side that the pursuer is currently on
theta = gps.gps_bearing(self._xp[0], self._xp[1], \
self._xt[0], self._xt[1])
offset_dir = \
gps.signum(gps.normalize_pi(theta + math.pi - self._xt[2]))
tgt_to_VT = self._xt[2] + math.atan2(PNInterceptor.D1 * offset_dir, \
PNInterceptor.D2)
self._VT = \
gps.gps_newpos(self._xt[0], self._xt[1], tgt_to_VT, \
math.hypot(PNInterceptor.D1, PNInterceptor.D2))
self._set_ctrl_params(self._xp, self._vp, \
(self._VT[0], self._VT[1], 0.0), (0.0, 0.0))
self._theta_VT_init = self._theta
self._theta_dot_VT_init = self._theta_dot
self._psi_p_VT_init = self._xp[2]
self._psi_p_t_f = gps.normalize(self._xt[2] + math.pi)
self._state_initialized = True
self._set_ctrl_params(self._xp, self._vp, \
(self._VT[0], self._VT[1], 0.0), (0.0, 0.0))
if self._range < PNInterceptor.R_CAP_PARALLEL:
self._state_initialized = False
self._state = PNInterceptor.PARALLEL_1
self.log_info("Transition to parallel_1 phase")
# Determine how to maneuver to the virtual target point
# Direct turn to the virtual target
if (self._theta_VT_init <= self._psi_p_t_f) and \
(self._psi_p_t_f <= ((2.0 * self._theta_VT_init) - self._psi_p_VT_init)):
self._gain = math.fabs((self._psi_p_t_f - self._psi_p_VT_init) / \
(self._psi_p_t_f - self._theta_VT_init))
self._bias = 0.0
# Single turn to virtual target with 2 gains
elif ((-math.pi - self._psi_p_VT_init + \
(2.0 * self._theta_VT_init)) <= self._psi_p_t_f) and \
(self._psi_p_t_f <= ((2.0 * self._theta_VT_init) - \
self._psi_p_VT_init)):
if (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) < 2.0:
self._gain = (2.0 / math.pi) * self._psi_p_VT_init
self._bias = 0.0
elif (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) == 2.0:
self._gain = 2.0
self._bias = 0.0
else: # should never get here
self.log_warn("Unable to determine phase 2 2-step case")
self.set_ready_state(False)
# S-turn to virtual target
else:
if gps.signum(self._theta_dot) == \
gps.signum(self._theta_dot_VT_init):
self._gain = 2.5
self._bias = 0.1 * gps.signum(self._theta_dot_VT_init)
elif (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) < 2.0:
self._gain = 2.5
self._bias = 0.05 * gps.signum(self._theta_dot_VT_init)
elif (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) >= 2.0:
self._gain = math.fabs((self._psi_p_t_f - self._xp[2]) / \
(self._psi_p_t_f - self._theta))
self._bias = 0.0
else: # should never get here
self.log_warn("Unable to determine phase 2 3-step case")
self.set_ready_state(False)
def _offset_state(self):
''' Performs calculations and operations required for the "offset" state
'''
if not self._state_initialized:
# Set the virtual waypoint D2 meters along the target's path and
# offset by D1 meters to the side that the pursuer is currently on
theta = gps.gps_bearing(self._xp[0], self._xp[1], \
self._xt[0], self._xt[1])
offset_dir = \
gps.signum(gps.normalize_pi(theta + math.pi - self._xt[2]))
tgt_to_VT = self._xt[2] + math.atan2(PNInterceptor.D1 * offset_dir, \
PNInterceptor.D2)
self._VT = \
gps.gps_newpos(self._xt[0], self._xt[1], tgt_to_VT, \
math.hypot(PNInterceptor.D1, PNInterceptor.D2))
self._set_ctrl_params(self._xp, self._vp, \
(self._VT[0], self._VT[1], 0.0), (0.0, 0.0))
self._theta_VT_init = self._theta
self._theta_dot_VT_init = self._theta_dot
self._psi_p_VT_init = self._xp[2]
self._psi_p_t_f = gps.normalize(self._xt[2] + math.pi)
self._state_initialized = True
self._set_ctrl_params(self._xp, self._vp, \
(self._VT[0], self._VT[1], 0.0), (0.0, 0.0))
if self._range < PNInterceptor.R_CAP_PARALLEL:
self._state_initialized = False
self._state = PNInterceptor.PARALLEL_1
self.log_info("Transition to parallel_1 phase")
# Determine how to maneuver to the virtual target point
# Direct turn to the virtual target
if (self._theta_VT_init <= self._psi_p_t_f) and \
(self._psi_p_t_f <= ((2.0 * self._theta_VT_init) - self._psi_p_VT_init)):
self._gain = math.fabs((self._psi_p_t_f - self._psi_p_VT_init) / \
(self._psi_p_t_f - self._theta_VT_init))
self._bias = 0.0
# Single turn to virtual target with 2 gains
elif ((-math.pi - self._psi_p_VT_init + \
(2.0 * self._theta_VT_init)) <= self._psi_p_t_f) and \
(self._psi_p_t_f <= ((2.0 * self._theta_VT_init) - \
self._psi_p_VT_init)):
if (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) < 2.0:
self._gain = (2.0 / math.pi) * self._psi_p_VT_init
self._bias = 0.0
elif (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) == 2.0:
self._gain = 2.0
self._bias = 0.0
else: # should never get here
self.log_warn("Unable to determine phase 2 2-step case")
self.set_ready_state(False)
# S-turn to virtual target
else:
if gps.signum(self._theta_dot) == \
gps.signum(self._theta_dot_VT_init):
self._gain = 2.5
self._bias = 0.1 * gps.signum(self._theta_dot_VT_init)
elif (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) < 2.0:
self._gain = 2.5
self._bias = 0.05 * gps.signum(self._theta_dot_VT_init)
elif (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) >= 2.0:
self._gain = math.fabs((self._psi_p_t_f - self._xp[2]) / \
(self._psi_p_t_f - self._theta))
self._bias = 0.0
else: # should never get here
self.log_warn("Unable to determine phase 2 3-step case")
self.set_ready_state(False)