def decode_servos_fixed_wing(self, servos):
'''decode servos for fixed wing'''
roll_rate = util.constrain(servos[0] - 0.5, -0.5, 0.5)
pitch_rate = util.constrain(servos[1] - 0.5, -0.5, 0.5)
throttle = util.constrain(servos[2], 0, 1)
yaw_rate = util.constrain(servos[3] - 0.5, -0.5, 0.5)
buf = struct.pack('<fffff', roll_rate, pitch_rate, throttle, yaw_rate,
0)
self.sim.send(buf)
def decode_servos_fixed_wing(self, servos):
'''decode servos for fixed wing'''
roll_rate = util.constrain(servos[0]-0.5, -0.5, 0.5)
pitch_rate = util.constrain(servos[1]-0.5, -0.5, 0.5)
throttle = util.constrain(servos[2], 0, 1)
yaw_rate = util.constrain(servos[3]-0.5, -0.5, 0.5)
buf = struct.pack('<fffff',
roll_rate,
pitch_rate,
throttle,
yaw_rate,
0)
self.sim.send(buf)
def __call__(self, V):
"""
:params V: a numpy array of size J x d (data matrix)
:returns (J x d) numpy array representing witness evaluations at the J
points.
"""
J = V.shape[0]
X = self.dat.data()
n, d = X.shape
# construct the feature tensor (n x d x J)
fssd = FSSD(self.p, self.k, V, null_sim=None, alpha=None)
# When X, V contain many points, this can use a lot of memory.
# Process chunk by chunk.
block_rows = util.constrain(50000 // (d * J), 10, 5000)
avg_rows = []
for (f, t) in util.ChunkIterable(start=0, end=n,
chunk_size=block_rows):
assert f < t
Xblock = X[f:t, :]
b = Xblock.shape[0]
F = fssd.feature_tensor(Xblock)
Tau = np.reshape(F, [b, d * J])
# witness evaluations computed on only a subset of data
avg_rows.append(Tau.mean(axis=0))
# an array of length d*J
witness_evals = (float(b) / n) * np.sum(np.vstack(avg_rows), axis=0)
assert len(witness_evals) == d * J
return np.reshape(witness_evals, [J, d])
def decode_servos_heli(self, servos):
'''decode servos for heli'''
swash1 = servos[0]
swash2 = servos[1]
swash3 = servos[2]
tail_rotor = servos[3]
rsc = servos[7]
col_pitch = (swash1 + swash2 + swash3) / 3.0 - 0.5
roll_rate = (swash1 - swash2) / 2
pitch_rate = -((swash1 + swash2) / 2.0 - swash3) / 2
yaw_rate = -(tail_rotor - 0.5)
rsc_speed = rsc
buf = struct.pack('<fffff', util.constrain(roll_rate, -0.5, 0.5),
util.constrain(pitch_rate, -0.5, 0.5),
util.constrain(rsc, 0, 1),
util.constrain(yaw_rate, -0.5, 0.5),
util.constrain(col_pitch, -0.5, 0.5))
self.sim.send(buf)
def decode_servos_heli(self, servos):
'''decode servos for heli'''
swash1 = servos[0]
swash2 = servos[1]
swash3 = servos[2]
tail_rotor = servos[3]
rsc = servos[7]
col_pitch = (swash1+swash2+swash3)/6.0
roll_rate = (swash1 - swash2)/2
pitch_rate = -((swash1 + swash2)/2.0 - swash3)/2
yaw_rate = -(tail_rotor - 0.5)
rsc_speed = rsc
buf = struct.pack('<fffff',
util.constrain(roll_rate, -0.5, 0.5),
util.constrain(pitch_rate, -0.5, 0.5),
util.constrain(rsc, 0, 1),
util.constrain(yaw_rate, -0.5, 0.5),
util.constrain(col_pitch, -0.5, 0.5))
self.sim.send(buf)
def get_drive(self):
l = deadzone(self.j.get_axis(self.LEFT), .25) * 5000
r = deadzone(self.j.get_axis(self.RIGHT), .25) * 5000
l = constrain(l, 4095)
r = constrain(r, 4095)
zf, zr = 0, 0
hat = self.j.get_hat(0)[1]
if hat:
zf = -4095 * hat
zr = 4095 * hat
elif self.j.get_button(self.UP):
zf = zr = -4095
elif self.j.get_button(self.DOWN):
zf = zr = 4095
claw = 0
if self.j.get_button(self.CLAW_O):
claw = CLAW_SPEED
elif self.j.get_button(self.CLAW_C):
claw = -CLAW_SPEED
return -r, -l, zf, zr, claw
def update(self):
'''update the gimbal state'''
# calculate delta time in seconds
now = time.time()
delta_t = now - self.last_update_t
self.last_update_t = now
if self.dcm is None:
# start with copter rotation matrix
self.dcm = self.vehicle.dcm.copy()
# take a copy of the demanded rates to bypass the limiter function for testing
demRateRaw = self.demanded_angular_rate
# 1) Rotate the copters rotation rates into the gimbals frame of reference
# copterAngRate_G = transpose(DCMgimbal)*DCMcopter*copterAngRate
copterAngRate_G = self.dcm.transposed()*self.vehicle.dcm*self.vehicle.gyro
#print("copterAngRate_G ", copterAngRate_G)
# 2) Subtract the copters body rates to obtain a copter relative rotational
# rate vector (X,Y,Z) in gimbal sensor frame
# relativeGimbalRate(X,Y,Z) = gimbalRateDemand - copterAngRate_G
relativeGimbalRate = self.demanded_angular_rate - copterAngRate_G
#print("relativeGimbalRate ", relativeGimbalRate)
# calculate joint angles (euler312 order)
# calculate copter -> gimbal rotation matrix
rotmat_copter_gimbal = self.dcm.transposed() * self.vehicle.dcm
self.joint_angles = Vector3(*rotmat_copter_gimbal.transposed().to_euler312())
#print("joint_angles ", self.joint_angles)
# 4) For each of the three joints, calculate upper and lower rate limits
# from the corresponding angle limits and current joint angles
#
# upperRatelimit = (jointAngle - lowerAngleLimit) * travelLimitGain
# lowerRatelimit = (jointAngle - upperAngleLimit) * travelLimitGain
#
# travelLimitGain is equal to the inverse of the bump stop time constant and
# should be set to something like 20 initially. If set too high it can cause
# the rates to 'ring' when they the limiter is in force, particularly given
# we are using a first order numerical integration.
upperRatelimit = -(self.joint_angles - self.upper_joint_limits) * self.travelLimitGain
lowerRatelimit = -(self.joint_angles - self.lower_joint_limits) * self.travelLimitGain
# 5) Calculate the gimbal joint rates (roll, elevation, azimuth)
#
# gimbalJointRates(roll, elev, azimuth) = Matrix*relativeGimbalRate(X,Y,Z)
#
# where matrix =
#
# +- -+
# | cos(elevAngle), 0, sin(elevAngle) |
# | |
# | sin(elevAngle) tan(rollAngle), 1, -cos(elevAngle) tan(rollAngle) |
# | |
# | sin(elevAngle) cos(elevAngle) |
# | - --------------, 0, -------------- |
# | cos(rollAngle) cos(rollAngle) |
# +- -+
rollAngle = self.joint_angles.x
elevAngle = self.joint_angles.y
matrix = Matrix3(Vector3(cos(elevAngle), 0, sin(elevAngle)),
Vector3(sin(elevAngle)*tan(rollAngle), 1, -cos(elevAngle)*tan(rollAngle)),
Vector3(-sin(elevAngle)/cos(rollAngle), 0, cos(elevAngle)/cos(rollAngle)))
gimbalJointRates = matrix * relativeGimbalRate
#print("lowerRatelimit ", lowerRatelimit)
#print("upperRatelimit ", upperRatelimit)
#print("gimbalJointRates ", gimbalJointRates)
# 6) Apply the rate limits from 4)
gimbalJointRates.x = util.constrain(gimbalJointRates.x, lowerRatelimit.x, upperRatelimit.x)
gimbalJointRates.y = util.constrain(gimbalJointRates.y, lowerRatelimit.y, upperRatelimit.y)
gimbalJointRates.z = util.constrain(gimbalJointRates.z, lowerRatelimit.z, upperRatelimit.z)
# 7) Convert the modified gimbal joint rates to body rates (still copter
# relative)
# relativeGimbalRate(X,Y,Z) = Matrix * gimbalJointRates(roll, elev, azimuth)
#
# where Matrix =
#
# +- -+
# | cos(elevAngle), 0, -cos(rollAngle) sin(elevAngle) |
# | |
# | 0, 1, sin(rollAngle) |
# | |
# | sin(elevAngle), 0, cos(elevAngle) cos(rollAngle) |
# +- -+
matrix = Matrix3(Vector3(cos(elevAngle), 0, -cos(rollAngle)*sin(elevAngle)),
Vector3(0, 1, sin(rollAngle)),
Vector3(sin(elevAngle), 0, cos(elevAngle)*cos(rollAngle)))
relativeGimbalRate = matrix * gimbalJointRates
# 8) Add to the result from step 1) to obtain the demanded gimbal body rates
# in an inertial frame of reference
# demandedGimbalRatesInertial(X,Y,Z) = relativeGimbalRate(X,Y,Z) + copterAngRate_G
demandedGimbalRatesInertial = relativeGimbalRate + copterAngRate_G
#if now - self.last_print_t >= 1.0:
# print("demandedGimbalRatesInertial ", demandedGimbalRatesInertial)
# for the moment we will set gyros equal to demanded_angular_rate
self.gimbal_angular_rate = demRateRaw #demandedGimbalRatesInertial + self.true_gyro_bias - self.supplied_gyro_bias
# update rotation of the gimbal
self.dcm.rotate(self.gimbal_angular_rate*delta_t)
self.dcm.normalize()
# calculate copter -> gimbal rotation matrix
rotmat_copter_gimbal = self.dcm.transposed() * self.vehicle.dcm
# calculate joint angles (euler312 order)
self.joint_angles = Vector3(*rotmat_copter_gimbal.transposed().to_euler312())
# update observed gyro
self.gyro = self.gimbal_angular_rate + self.true_gyro_bias
# update delta_angle (integrate)
self.delta_angle += self.gyro * delta_t
# calculate accel in gimbal body frame
copter_accel_earth = self.vehicle.dcm * self.vehicle.accel_body
accel = self.dcm.transposed() * copter_accel_earth
# integrate velocity
self.delta_velocity += accel * delta_t
# see if we should do a report
if now - self.last_report_t >= 1.0 / self.reporting_rate:
self.send_report()
self.last_report_t = now
if now - self.last_print_t >= 1.0:
self.last_print_t = now
# calculate joint angles (euler312 order)
Euler312 = Vector3(*self.dcm.to_euler312())
print("Euler Angles 312 %6.1f %6.1f %6.1f" % (degrees(Euler312.z), degrees(Euler312.x), degrees(Euler312.y)))
def shoot(self, shooter, target):
if not shooter.can_shoot:
print('unit is not permitted to shoot right now')
return
elif target.quantity <= 0 or shooter.quantity <= 0:
print 'error, one unit is empty / wiped out'
return
elif target.allegiance == shooter.allegiance:
print('(was gonna shoot but didnt because both units are from same army)')
return
elif self.is_locked(shooter) or self.is_locked(target):
print('was gonna shoot but shooter or target is locked in close combat.')
return
# more assert eg closest-target Ld check TODO
dist = util.distance(shooter.coord, target.coord)
if shooter.rng < dist:
print(
'Tried to shoot but target is out of range ({real}sq / {realin}", ' +
'range: {max}sq / {maxin}")').format(
real=dist, realin=util.inches(dist),
max=shooter.rng, maxin=util.inches(shooter.rng))
return
print shooter.name_with_sprite() + ' is shooting at ' + target.name_with_sprite()
shooter.can_shoot = False
print ' ' + util.conjugateplural(shooter.quantity, "shooter") + "..."
totalshotcount = shooter.shotspercreature * shooter.quantity
attacks = []
for i in range(totalshotcount):
attacks.append(Attack(shooter.shootstr, shooter.ap))
print ' ' + util.conjugateplural(len(attacks), "shot") + "..."
# To Hit
for attack in attacks:
if not attack.active:
continue
need_to_roll = 7 - shooter.bs
need_to_roll = util.constrain(need_to_roll, 2, 6)
result = util.roll(
'Shooting to-hit roll, shooter needs a {goal}+'.format(goal = need_to_roll),
successif = (lambda n: n>=need_to_roll))
if result < need_to_roll:
attack.active = False
# Attacks that do not hit, wound, etc are marked .active = False
# And then removed. Might be clumsy.
hits = [a for a in attacks if a.active]
print ' ' + util.conjugateplural(len(hits), "hit") + "..."
# To Wound
for attack in hits:
need_to_roll = target.t - shooter.shootstr + 4
need_to_roll = util.constrain(need_to_roll, 2, 6)
result = util.roll(
'Shooting to-wound roll, shooter needs a {goal}+'.format(goal = need_to_roll),
successif = (lambda n: n>=need_to_roll))
if result < need_to_roll:
attack.active = False
wounds = [a for a in hits if a.active]
print ' ' + util.conjugateplural(len(wounds), "pre-saving wound") + "..."
# Saves
for attack in wounds:
# TODO: invulnerable saves, cover saves
if target.sv >= 7 or target.sv >= attack.ap:
break
need_to_roll = target.sv
result = util.roll(
'Shooting armor save, target saves on a {goal}+'.format(goal = need_to_roll))
if result >= need_to_roll:
attack.active = False
unsaved_wounds = [a for a in wounds if a.active]
# print final results summary
print ' {n} casualties...'.format(n=len(unsaved_wounds))
if len(unsaved_wounds) > 0:
# TODO bug, 1 unsaved wound led to multiple lost models. seen once 2015 aug 9.
self.damage(target, len(unsaved_wounds))
print ' Now target has ' + str(target.quantity) + ' creatures left in it.'
