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.'