def update(self, char):
"""
Updates the puzzle given the character.
:param char: the character to emulate.
"""
y, x = self.cursor_pos
if char == curses.KEY_RIGHT:
x += 1
elif char == curses.KEY_LEFT:
x -= 1
elif char == curses.KEY_UP:
y -= 1
elif char == curses.KEY_DOWN:
y += 1
elif char in (ord(' '), ord('z')):
self.mark(x, y)
elif char == ord('x'):
self.discard(x, y)
elif char == ord('c'):
self.count_distance = not self.count_distance
self.count_origin = tuple(self.cursor_pos)
x = utils.clamp(x, 0, self.level.width - 1)
y = utils.clamp(y, 0, self.level.height - 1)
self.cursor_pos = (y, x)
def tick(self, dtime):
self.keyboard.capture()
selectedEnt2 = self.ent.engine.selectionMgr.selectedEnts[1]
if self.toggle >= 0:
self.toggle -= dtime
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_W):
self.toggle = 0.15
selectedEnt2.desiredSpeed = utils.clamp(
selectedEnt2.desiredSpeed + selectedEnt2.deltaSpeed, 0,
selectedEnt2.maxSpeed)
# Slower
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_S):
self.toggle = 0.15
selectedEnt2.desiredSpeed = utils.clamp(
selectedEnt2.desiredSpeed - selectedEnt2.deltaSpeed / 5.5, 0,
selectedEnt2.maxSpeed)
# turn left
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_A):
self.toggle = 0.15
selectedEnt2.desiredHeading += selectedEnt2.deltaYaw
#print "Control: ", str(selectedEnt), str(selectedEnt.desiredHeading)
# turn right
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_D):
self.toggle = 0.15
selectedEnt2.desiredHeading -= selectedEnt2.deltaYaw
#print "Control: ", str(selectedEnt), str(selectedEnt.desiredHeading)
selectedEnt2.desiredHeading = utils.fixAngle(
selectedEnt2.desiredHeading)
def handle_yt_sphere_update(self, request, session, data):
user = self.flaskserver.get_user_by_request(request, session)
if user.room is None:
return
# does not work well with peer-sync mode, only allow in host-sync mode
if not user.room.is_creator(user):
return
yaw = self.getval(
data, 'properties.yaw',
lambda x: isinstance(x, float) and x >= 0 and x < 360,
lambda x: clamp(float(x), 0, 360), 0)
pitch = self.getval(
data, 'properties.pitch',
lambda x: isinstance(x, float) and x >= -90 and x <= 90,
lambda x: clamp(float(x), -90, 90), 0)
roll = self.getval(
data, 'properties.roll',
lambda x: isinstance(x, float) and x >= -180 and x <= 180,
lambda x: clamp(float(x), -180, 180), 0)
fov = self.getval(
data, 'properties.fov',
lambda x: isinstance(x, float) and x >= 30 and x <= 120,
lambda x: clamp(float(x), 30, 120), 100)
user.room.emit(EVENT_YT_SPHERE_UPDATE, {
'properties': {
'yaw': yaw,
'pitch': pitch,
'roll': roll,
'fov': fov
}
},
sender=user)
def _optimal_midfielder_pos(state) -> Coordinate:
side = 1 if state.world_view.side == 'l' else -1
if not state.world_view.ball.is_value_known():
return state.get_global_play_pos()
ball: Coordinate = state.world_view.ball.get_value().coord
play_position = state.get_global_play_pos()
ball_delta_y = ball.pos_y - play_position.pos_y
ball_delta_x = ball.pos_x - play_position.pos_x
# Position player according to their starting position and the current ball position
if side * ball.pos_x > 0:
# Attacking
optimal_x = clamp(
play_position.pos_x + ball_delta_x * 0.4 + ball.pos_x * 0.6, -45,
45)
else:
# Defending
optimal_x = clamp(play_position.pos_x + ball.pos_x * 0.4, -45, 45)
# Used to make players position themselves closer to the goal on the y-axis when far up/down the field
y_goal_factor = 1 - (abs(optimal_x) -
35) * 0.05 if abs(optimal_x) > 35 else 1.0
optimal_y = clamp(
play_position.pos_y + ball_delta_y * 0.2 + ball.pos_y * 0.2, -25,
25) * y_goal_factor
if state.world_view.team_has_ball(state.team_name, max_data_age=4):
opt_coord = Coordinate(optimal_x + (10 * side), optimal_y)
free_pos = state.get_closest_free_position(opt_coord)
if state.is_test_player():
debug_msg("Free position:{0}".format(free_pos), "FREE_POSITION")
return opt_coord if free_pos is None else free_pos
return Coordinate(optimal_x, optimal_y)
def faceDetected(boxes):
evaluate("faceDetected")
if isFlagRaised("Camera track") and not isFlagRaised("Body track"):
face_left = boxes.faces[0].top_left.x
face_top = boxes.faces[0].top_left.y
face_width = boxes.faces[0].width_height.x
face_height = boxes.faces[0].width_height.y
# Calculate correction
ideal_left = 0.5 - face_width / 2.0
correction_pan = (ideal_left - face_left) * 2.5
ideal_top = 0.5 - face_height / 2.0
correction_tilt = (ideal_top - face_top) * 2.5
temp1=angles["pan"][-1] + correction_pan
temp2=angles["tilt"][-1]- correction_tilt
# Clamp pan and tilt angles
angles["pan"].append(clamp(0, temp1, 90))
angles["tilt"].append(clamp(0, temp2, 90))
# Publish new_angles
pan_head(angles["pan"][-1])
tilt_head(angles["tilt"][-1])
clearFlags()
def get_surrounding_pixels(source,pixels_shape, radius=5):
x,y = source
x_flr = int(x)
y_flr = int(y)
offset = radius + 1
radius_squared = radius**2
y_min = clamp(y_flr - offset, 0, pixels_shape[0] - 1)
y_max = clamp(y_flr + offset, 0, pixels_shape[0] - 1)
x_min = clamp(x_flr - offset, 0, pixels_shape[1] - 1)
x_max = clamp(x_flr + offset, 0, pixels_shape[1] - 1)
subgrid_shape = (y_max - y_min + 1, x_max - x_min + 1)
indices = numpy.empty((81,), dtype=[('x', int), ('y', int)])
idx = 0
#print(f"S shape {subgrid_shape[0], subgrid_shape[1]}")
for pixel_y in range(0, subgrid_shape[0]):
for pixel_x in range(0, subgrid_shape[1]):
img_y = y_min + pixel_y + 0.5
img_x = x_min + pixel_x + 0.5
dist_squared = (img_x - x)**2 + (img_y - y)**2
if dist_squared <= radius_squared:
indices[idx] = (img_x - 0.5, img_y -0.5)
idx += 1
return indices[:idx]
def tick(self, dt):
# Update the toggle timer.
if self.toggle >= 0:
self.toggle -= dt
import utils
entList = self.selMgr.getSelected()
if entList:
for selectedEnt in entList:
#change desired speed and current direction
#------------------------------------------
# Speed Up
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_UP):
selectedEnt.desiredSpeed = utils.clamp(selectedEnt.desiredSpeed + selectedEnt.deltaSpeed, 0, selectedEnt.maxSpeed)
print "Speeding UP", selectedEnt.desiredSpeed
# Slow down
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_DOWN):
selectedEnt.desiredSpeed = utils.clamp(selectedEnt.desiredSpeed - selectedEnt.deltaSpeed, 0, selectedEnt.maxSpeed)
print "Slowing down", selectedEnt.desiredSpeed
# Turn Left.
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_LEFT):
selectedEnt.desiredHeading += selectedEnt.deltaYaw
selectedEnt.desiredHeading = utils.fixAngle(selectedEnt.desiredHeading)
print "Turn left", selectedEnt.desiredHeading
# Turn Right.
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_RIGHT):
selectedEnt.desiredHeading -= selectedEnt.deltaYaw
selectedEnt.desiredHeading = utils.fixAngle(selectedEnt.desiredHeading)
print "Turn right", selectedEnt.desiredHeading
if self.toggle < 0:
toggle = 0.2
pass
def get_output(self, current_input: float, setpoint: float) -> float:
'''Get PID output for process
current_input:
The current PID input
setpoint:
Desired output of process/input to PID
'''
# Current time in seconds
current_time = Timer.getFPGATimestamp()
# Time elapsed since last update
time_change = current_time - self._previous_time
# The current error
current_error = self._get_continuous_error(setpoint - current_input)
self._integral_term += self._coefs.i * (current_error * time_change)
self._integral_term = clamp(self._integral_term, self._output_max,
self._output_min)
# Protect againsts ZeroDivisionError caused
# by time resolution in simulator
if time_change <= 0.0:
time_change = 0.005
derivative = (current_input - self._previous_input) / time_change
self._previous_input = current_input
self._previous_time = current_time
output = ((self._coefs.p * current_error) + self._integral_term +
(self._coefs.d * derivative))
return clamp(output, self._output_max, self._output_min)
def get_next_cell(w, state, action):
sx = int(state % w.width)
sy = int(state / w.width)
oldx = sx
oldy = sy
if action == world.ACTION_DOWN:
sy += 1
elif action == world.ACTION_LEFT:
sx -= 1
elif action == world.ACTION_RIGHT:
sx += 1
elif action == world.ACTION_UP:
sy -= 1
sx = clamp(sx, 0, w.width - 1)
sy = clamp(sy, 0, w.height - 1)
if w.get_cell(sx, sy).type == world.CELL_WALL:
sx = oldx
sy = oldy
return sy * w.width + sx
def tick(self, dt):
if self.engine.keepRunning == True:
self.keyboard.capture()
if self.toggle >= 0:
self.toggle -= dt
if self.toggle < 0:
self.toggle = 0.1
for entity in self.engine.selectionMgr.selectedEnts:
if self.keyboard.isKeyDown(OIS.KC_UP):
entity.desiredSpeed = utils.clamp(entity.desiredSpeed + entity.deltaSpeed, 0, entity.maxSpeed)
print "Speeding UP", entity.desiredSpeed
if self.keyboard.isKeyDown(OIS.KC_DOWN):
entity.desiredSpeed = utils.clamp(entity.desiredSpeed - entity.deltaSpeed, 0, entity.maxSpeed)
print "Slowing DOWN", entity.desiredSpeed
if self.keyboard.isKeyDown(OIS.KC_LEFT):
entity.desiredHeading += entity.deltaYaw
entity.desiredHeading = utils.fixAngle(entity.desiredHeading)
print "Turn left", ogre.Radian(entity.desiredHeading).valueDegrees()
if self.keyboard.isKeyDown(OIS.KC_RIGHT):
entity.desiredHeading -= entity.deltaYaw
entity.desiredHeading = utils.fixAngle(entity.desiredHeading)
print "Turn right", ogre.Radian(entity.desiredHeading).valueDegrees()
if self.keyboard.isKeyDown(OIS.KC_SPACE):
entity.desiredSpeed = 0
def update(self, delta_time, scene):
super().update(delta_time)
# Move to the left
if self.key_left:
# Move the shooter
self.arc_angle = clamp(self.arc_angle + 0.09 * delta_time, self.min_angle, self.max_angle)
# Move the gear
if self.arc_angle < self.max_angle:
self.polygon_smooth += 0.25 * delta_time
# Move to the right
elif self.key_right:
# Move the shooter
self.arc_angle = clamp(self.arc_angle - 0.09 * delta_time, self.min_angle, self.max_angle)
# Move the gear
if self.arc_angle > self.min_angle:
self.polygon_smooth -= 0.25 * delta_time
# Shoot
if self.key_shoot and not scene.score and self.cooldown == 0:
# Reset cooldown
self.cooldown = self.max_cooldown
# Instantiate the projectile
scene.projectiles.add(Projectile(self.x, self.y, self.color, -(self.arc_angle + 13.5), 20))
# Play a shooting sound
random.choice(self.snd_shoot).play()
def tick(self, dtime):
self.keyboard.capture()
selectedEnt2 = self.ent.engine.selectionMgr.selectedEnts[1]
if self.toggle >= 0:
self.toggle -= dtime
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_W):
self.toggle = 0.15
selectedEnt2.desiredSpeed = utils.clamp(selectedEnt2.desiredSpeed + selectedEnt2.deltaSpeed, 0, selectedEnt2.maxSpeed)
# Slower
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_S):
self.toggle = 0.15
selectedEnt2.desiredSpeed = utils.clamp(selectedEnt2.desiredSpeed - selectedEnt2.deltaSpeed/5.5, 0, selectedEnt2.maxSpeed)
# turn left
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_A):
self.toggle = 0.15
selectedEnt2.desiredHeading += selectedEnt2.deltaYaw
#print "Control: ", str(selectedEnt), str(selectedEnt.desiredHeading)
# turn right
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_D):
self.toggle = 0.15
selectedEnt2.desiredHeading -= selectedEnt2.deltaYaw
#print "Control: ", str(selectedEnt), str(selectedEnt.desiredHeading)
selectedEnt2.desiredHeading = utils.fixAngle(selectedEnt2.desiredHeading)
def setPixelColor(self, pixel, red, green, blue):
""" Set specified LED to RGB value """
if pixel < LEDShow.LED_COUNT:
red = utils.clamp(red, 0, 255)
green = utils.clamp(green, 0, 255)
blue = utils.clamp(blue, 0, 255)
self.strip.setPixelColorRGB(pixel, green, red, blue) #GRB to RGB
def _process(self, delta):
velocity = Vector2()
if Input.is_action_pressed("ui_right"):
velocity.x += 1
if Input.is_action_pressed("ui_left"):
velocity.x -= 1
if Input.is_action_pressed("ui_down"):
velocity.y += 1
if Input.is_action_pressed("ui_up"):
velocity.y -= 1
animated_sprite = self.get_node("AnimatedSprite")
if velocity.length() > 0:
velocity = velocity.normalized() * self.speed
animated_sprite.play()
else:
animated_sprite.stop()
self.position += velocity * delta
# NOTE: It seems you can't set the self.position's x and y values
# so you'll need to assign a new vector instead.
self.position = Vector2(
clamp(self.position.x, 0, self.screen_size.x),
clamp(self.position.y, 0, self.screen_size.y),
)
if velocity.x != 0:
animated_sprite.animation = "walk"
animated_sprite.flip_v = False
animated_sprite.flip_h = velocity.x < 0
elif velocity.y != 0:
animated_sprite.animation = "up"
animated_sprite.flip_v = velocity.y > 0
def tick(self, dtime):
#----------position-----------------------------------
timeScaledAcceleration = self.ent.acceleration * (dtime * 100)
self.ent.speed += utils.clamp(self.ent.desiredSpeed - self.ent.speed,
-timeScaledAcceleration,
timeScaledAcceleration)
#self.ent.vel.x = math.cos(-self.ent.heading) * self.ent.speed
#self.ent.vel.z = math.sin(-self.ent.heading) * self.ent.speed
#self.ent.vel.y = 0
self.ent.vel = Vector3(self.ent.speed * math.cos(-self.ent.heading), 0,
self.ent.speed * math.sin(-self.ent.heading))
self.ent.pos = self.ent.pos + (self.ent.vel * dtime)
#------------heading----------------------------------
timeScaledRotation = self.ent.turningRate * (dtime * 20)
angleDiff = utils.diffAngle(self.ent.desiredHeading, self.ent.heading)
dheading = utils.clamp(angleDiff, -timeScaledRotation,
timeScaledRotation)
print self.ent.desiredHeading
print "meow"
print self.ent.heading
self.ent.heading += dheading
def set_velocity(self, linear_speed, angular_speed):
"""
Registers a robot velocity command. Since the actuation system is delayed, the command may not be
immediately executed.
:param linear_speed: the robot's linear speed.
:type linear_speed: float
:param angular_speed: the robot's angular speed.
:type angular_speed: float
"""
right_speed, left_speed = self.unicycle_to_wheels(
linear_speed, angular_speed)
right_speed = clamp(right_speed, -self.max_wheel_speed,
self.max_wheel_speed)
left_speed = clamp(left_speed, -self.max_wheel_speed,
self.max_wheel_speed)
linear, angular = self.wheels_to_unicycle(right_speed, left_speed)
if len(self.linear_speed_commands) >= self.delay:
self.reference_linear_speed = self.linear_speed_commands[-self.
delay]
if len(self.angular_speed_commands) >= self.delay:
self.reference_angular_speed = self.angular_speed_commands[-self.
delay]
self.linear_speed_commands.append(linear)
self.angular_speed_commands.append(angular)
if len(self.linear_speed_commands) > self.delay:
self.linear_speed_commands.pop(0)
if len(self.angular_speed_commands) > self.delay:
self.angular_speed_commands.pop(0)
def _optimal_defender_pos(state) -> Coordinate:
side = 1 if state.world_view.side == 'l' else -1
ball: Coordinate = state.world_view.ball.get_value().coord
play_position = state.get_global_play_pos()
ball_delta_y = ball.pos_y - play_position.pos_y
ball_delta_x = ball.pos_x - play_position.pos_x
# Position player according to their starting position and the current ball position
optimal_x = play_position.pos_x + ball_delta_x * 0.4 + ball.pos_x * 0.4
if side > 0:
optimal_x = clamp(optimal_x, -45, -3)
else:
optimal_x = clamp(optimal_x, 3, 45)
# Used to make players position themselves closer to the goal on the y-axis when far up/down the field
y_goal_factor = 1 - (abs(optimal_x) -
35) * 0.05 if abs(optimal_x) > 35 else 1.0
optimal_y = clamp(
play_position.pos_y + ball.pos_y * 0.4 + ball_delta_y * 0.1, -25,
25) * y_goal_factor
if state.world_view.team_has_ball(state.team_name, max_data_age=4):
return Coordinate(optimal_x + (10 * side), optimal_y)
return Coordinate(optimal_x, optimal_y)
def train_step(
X,
y,
model,
optimizer,
scheduler, # generic
reg_weight,
geometry,
epsilon,
alpha,
lower_limit,
upper_limit,
attack_iters,
criterion # experiment-specific
):
"""TRADES algorithm [https://arxiv.org/abs/1901.08573].
"""
### adversarial perturbation
# init perturbation
delta = torch.zeros_like(X)
for i in range(len(epsilon)):
delta[:, i, :, :].uniform_(-epsilon[i][0][0].item(),
epsilon[i][0][0].item())
delta.data = clamp(delta, lower_limit - X, upper_limit - X)
# find approx optimal perturbation
criterion_kl = nn.KLDivLoss(reduction='sum')
with toggle_eval(model): # turn off batchnorm stat tracking
for _ in range(attack_iters):
with toggle_requires_grad(delta, True):
grad = torch.autograd.grad(outputs=criterion_kl(
F.log_softmax(model(X + delta), dim=1),
F.softmax(model(X), dim=1)),
inputs=delta,
only_inputs=True)[0]
delta = project(delta + alpha * grad.sign(),
epsilon,
geometry=geometry)
delta = clamp(delta, lower_limit - X, upper_limit - X)
### adversarial loss
model.apply(zero_grad)
# disable batchnorm stat tracking to avoid distribution shift from adversarial examples
with disable_batchnorm_tracking(model):
loss = criterion_kl(F.log_softmax(model(X + delta), dim=1),
F.softmax(model(X), dim=1))
loss *= (reg_weight / X.shape[0])
loss.backward()
### clean loss
# batchnorm stat tracking is fine here
output = model(X)
loss = criterion(output, y)
### update model
loss.backward()
optimizer.step()
scheduler.step()
return loss, output
def __init__(self,
source, target,
source_toworld, target_toworld,
source_th=0, target_th=0,
source_mask=None, target_mask=None,
source_bins=256, target_bins=256):
"""
IconicMatcher class for intensity-based image registration.
"""
## FIXME: test that input images are 3d
# Source image binning
self.source = source
self.source_clamped, s_bins = clamp(source, th=source_th, mask=source_mask, bins=source_bins)
# Target image padding + binning
self.target = target
self.target_clamped = -np.ones(np.array(target.shape)+2, dtype=CLAMP_DTYPE)
aux, t_bins = clamp(target, th=target_th, mask=target_mask, bins=target_bins)
self.target_clamped[1:target.shape[0]+1:, 1:target.shape[1]+1:, 1:target.shape[2]+1:] = aux
# Histograms
self.joint_hist = np.zeros([s_bins, t_bins])
self.source_hist = np.zeros(s_bins)
self.target_hist = np.zeros(t_bins)
# Image-to-world transforms
self.source_toworld = source_toworld
self.target_fromworld = np.linalg.inv(target_toworld)
# Set default registration parameters
self.set_interpolation()
self.set_field_of_view()
self.set_similarity()
def objectDetected(tracked_object):
object_left = tracked_object.x
object_top = tracked_object.y
object_width = tracked_object.width
object_height = tracked_object.height
if object_left > 5 and object_top > 5:
# Calculate correction
ideal_left = 320 - object_width / 2.0
correction_pan = (ideal_left - object_left) / 50.00
ideal_top = 240 - object_height / 2.0
correction_tilt = (ideal_top - object_top)/ 50.00
angles["pan"] += correction_pan
angles["tilt"] -= correction_tilt
# Clamp pan and tilt angles
angles["pan"] = clamp(0, angles["pan"], 90)
angles["tilt"] = clamp(0, angles["tilt"], 90)
# Publish new values
pan_head(angles["pan"])
tilt_head(angles["tilt"])
# Move body
if (angles["pan"] < 40): # Move body to left
move_body("right")
elif (angles["pan"] > 50): # Move body to the right
move_body("left")
elif (getSonarDistance > 50):
move_forward()
def voxelize(self, px: ti.template(), world: ti.f32, n_grid: ti.i32,
bound_grid: ti.i32, npar: ti.i32):
dx = world / n_grid
bound = bound_grid * dx
space_x = dx / npar
for i, j, k in ti.ndrange(n_grid, n_grid, n_grid):
x = (i + 0.5) * dx
y = (j + 0.5) * dx
z = (k + 0.5) * dx
if (x - self.c_x)**2 + (y - self.c_y)**2 + (
z - self.c_z)**2 - self.r**2 < 0:
for ix in range(npar):
for jx in range(npar):
for kx in range(npar):
xp = clamp(
i * dx + (ix + random.random()) * space_x,
bound + 1e-4, world - bound - 1e-4)
yp = clamp(
j * dx + (jx + random.random()) * space_x,
bound + 1e-4, world - bound - 1e-4)
zp = clamp(
k * dx + (kx + random.random()) * space_x,
bound + 1e-4, world - bound - 1e-4)
px[particle_idx(i, j, k, ix, jx, kx, n_grid,
npar)] = vec3(xp, yp, zp)
def voxelize(self, px: ti.template(), world: ti.f32, n_grid: ti.i32,
bound_grid: ti.i32, npar: ti.i32):
dx = world / n_grid
bound = bound_grid * dx
space_x = dx / npar
for i, j, k in ti.ndrange(n_grid, n_grid, n_grid):
x = (i + 0.5) * dx
y = (j + 0.5) * dx
z = (k + 0.5) * dx
if self.x_start < x < self.x_end and self.y_start < y < self.y_end and self.z_start < z < self.z_end:
for ix in range(npar):
for jx in range(npar):
for kx in range(npar):
xp = clamp(
i * dx + (ix + random.random()) * space_x,
bound + 1e-4, world - bound - 1e-4)
yp = clamp(
j * dx + (jx + random.random()) * space_x,
bound + 1e-4, world - bound - 1e-4)
zp = clamp(
k * dx + (kx + random.random()) * space_x,
bound + 1e-4, world - bound - 1e-4)
px[particle_idx(i, j, k, ix, jx, kx, n_grid,
npar)] = vec3(xp, yp, zp)
def objectDetected(tracked_object):
object_left = tracked_object.x
object_top = tracked_object.y
object_width = tracked_object.width
object_height = tracked_object.height
if object_left > 5 and object_top > 5:
# Calculate correction
ideal_left = 320 - object_width / 2.0
correction_pan = (ideal_left - object_left) / 50.00
ideal_top = 240 - object_height / 2.0
correction_tilt = (ideal_top - object_top) / 50.00
angles["pan"] += correction_pan
angles["tilt"] -= correction_tilt
# Clamp pan and tilt angles
angles["pan"] = clamp(0, angles["pan"], 90)
angles["tilt"] = clamp(0, angles["tilt"], 90)
# Publish new values
pan_head(angles["pan"])
tilt_head(angles["tilt"])
# Move body
if (angles["pan"] < 40): # Move body to left
move_body("right")
elif (angles["pan"] > 50): # Move body to the right
move_body("left")
elif (getSonarDistance > 50):
move_forward()
def get_pixels_in_range(source, bound, pixels_shape):
x, y = source
x_flr = int(source[0])
y_flr = int(source[1])
lo_bound, hi_bound = bound
lo_squared, hi_squared = lo_bound**2, hi_bound**2
offset = hi_bound + 1
#print(pixels_shape)
y_min = clamp(y_flr - offset, 0, pixels_shape[0] - 1)
y_max = clamp(y_flr + offset, 0, pixels_shape[0] - 1)
x_min = clamp(x_flr - offset, 0, pixels_shape[1] - 1)
x_max = clamp(x_flr + offset, 0, pixels_shape[1] - 1)
subgrid_shape = (y_max - y_min + 1, x_max - x_min + 1)
indices = numpy.empty((448, ), dtype=[('x', int), ('y', int)])
#print(pixel_values)
idx = 0
for pixel_y in range(0, subgrid_shape[0]):
for pixel_x in range(0, subgrid_shape[1]):
img_y = y_min + pixel_y
img_x = x_min + pixel_x
dist_squared = (img_x + 0.5 - x)**2 + (img_y + 0.5 - y)**2
if dist_squared <= hi_squared and dist_squared >= lo_squared:
indices[idx] = (img_x, img_y)
idx += 1
return indices[:idx]
def sample(self, qf, u, v):
# assure integer
i, j = int(u), int(v)
# clamp
i = clamp(i, 0, self.cfg.res[0] - 1)
j = clamp(j, 0, self.cfg.res[1] - 1)
return qf[i, j]
def limit_velocity(self):
linvel = self.body.linear_velocity
angvel = self.body.angular_velocity
maxlinvel = self.max_linear_velocity
maxangvel = self.max_angular_velocity
vx = clamp(linvel.x, -maxlinvel, maxlinvel)
vy = clamp(linvel.y, -maxlinvel, maxlinvel)
self.body.linear_velocity = Vec2(vx, vy)
self.body.angular_velocity = clamp(angvel, -maxangvel, maxangvel)
def drawSquareAtCenter(self, x, y, size, color):
midpoint = int(size / 2)
for w in range(size):
for h in range(size):
self.leds.setPixelColorXY(
utils.clamp(x + w - midpoint, 0, 7),
utils.clamp(y - h + midpoint, 0,
self.leds.getRowCount() - 1), color[0],
color[1], color[2])
def __init__(self):
self.x = clamp(
random.normalvariate(.5, .28) * settings.WIDTH - 50, 0,
settings.WIDTH - 50)
self.y = clamp(
random.normalvariate(.5, .28) * settings.HEIGHT - 50, 0,
settings.HEIGHT - 50)
self.edible = True
self.energy = settings.FOOD_ENERGY
def pos_to_stagger_idx(pos, stagger):
pos[0] = clamp(pos[0], stagger[0] * grid_x,
w - 1e-4 - grid_x + stagger[0] * grid_x)
pos[1] = clamp(pos[1], stagger[1] * grid_y,
h - 1e-4 - grid_y + stagger[1] * grid_y)
p_grid = pos / vec2(grid_x, grid_y) - stagger
I = ti.cast(ti.floor(p_grid), ti.i32)
return I, p_grid
def draw_point(self, x, y, color=None, wait_time=3, is_print_info=False):
if color is None:
color = clamp(x, 0, 1), random.random(), clamp(y, 0, 1)
if is_print_info:
print("color = {}".format(color))
self.go_to_position(x, y, is_print_info=is_print_info)
self.arm.set_color(*color)
self.time.sleep(wait_time)
def move(self, x, y, twist=0): ''' Attemps to move in the direction given by x, y. ''' x, y = utils.clamp(x, -1.0, 1.0), utils.clamp(y, -1.0, 1.0) twist = utils.clamp(twist, -1.0, 0.0) left = (y + x) right = (y - x) left = utils.clamp(left, -1.0, 1.0) right = utils.clamp(right, -1.0, 1.0) self.motor_left.set_speed(left) self.motor_right.set_speed(right)
def lerp(x, x0, x1, c0, c1):
"""Linear interpolation of 24-bit color values. Given a value x within
the range x0, x1 and colors c0, c1 this will return a color c that is
linearly interpolated within c0, c1 proportional to x within x0, x1.
"""
r0, g0, b0 = decompose(c0)
r1, g1, b1 = decompose(c1)
return compose(utils.clamp(int(utils.lerp(x, x0, x1, r0, r1)), 0, 255),
utils.clamp(int(utils.lerp(x, x0, x1, g0, g1)), 0, 255),
utils.clamp(int(utils.lerp(x, x0, x1, b0, b1)), 0, 255))
def attack_pgd(model: torch.nn.Module,
X: torch.Tensor,
y: torch.Tensor,
epsilon: torch.Tensor,
alpha: float,
lower_limit: torch.Tensor,
upper_limit: torch.Tensor,
attack_iters: int,
restarts: int,
opt: torch.optim.Optimizer = None) -> torch.Tensor:
"""Adapted from: https://github.com/locuslab/fast_adversarial
"""
device = next(model.parameters()).device
model.eval()
lower_limit = lower_limit.to(device)
upper_limit = upper_limit.to(device)
max_loss = torch.zeros(y.shape[0]).to(device)
max_delta = torch.zeros_like(X).to(device)
for zz in range(restarts):
delta = torch.zeros_like(X)
for i in range(len(epsilon)):
delta[:, i, :, :].uniform_(-epsilon[i][0][0].item(),
epsilon[i][0][0].item())
delta.data = clamp(delta, lower_limit - X, upper_limit - X)
delta.requires_grad = True
for _ in range(attack_iters):
output = model(X + delta)
index = torch.where(output.max(1)[1] == y)
if len(index[0]) == 0:
break
loss = F.cross_entropy(output, y)
if opt is not None:
loss.backward()
else:
loss.backward()
grad = delta.grad.detach()
d = delta[index[0], :, :, :]
g = grad[index[0], :, :, :]
d = clamp(d + alpha * torch.sign(g), -epsilon, epsilon)
d = clamp(d, lower_limit - X[index[0], :, :, :],
upper_limit - X[index[0], :, :, :])
delta.data[index[0], :, :, :] = d
delta.grad.zero_()
all_loss = F.cross_entropy(model(X + delta), y,
reduction='none').detach()
max_delta[all_loss >= max_loss] = delta.detach()[all_loss >= max_loss]
max_loss = torch.max(max_loss, all_loss)
return max_delta
def simulate(net, config):
robot, ball, goal = reset()
global fitness, total_steps, MAX_STEPS, reset_sim
for step in range(MAX_STEPS):
# calculate new net inputs
rotated_center = Vec2d(10.5, 0)
rotated_center.rotate(robot.angle)
rotated_center += robot.position
goal_pos = utils.avg(goal.a, goal.b)
ball_dist = utils.dist(rotated_center,
ball.position) # robot -> ball dist
goal_dist = utils.dist(ball.position, goal_pos) # ball -> goal dist
ball_dir, goal_dir = utils.get_angles(
rotated_center, ball.position, goal_pos) # inputs for neural net
fitness = utils.calculate_fitness(ball_dist, goal_dist,
robot_touched_ball)
# scale values for nn
#ball_dir = np.interp(ball_dir, [0, 360], [0.0, 1.0])
#goal_dir = np.interp(goal_dir, [0, 360], [0.0, 1.0])
#ball_dist = np.interp(ball_dist, [0, 303.6], [0.0, 1.0])
# get input from neural net here, need to calculate balldir and goaldir though
# NEW INPJTS SHOULD BE: fixed ball dir, fixed ball dist, fix goal direction, fixed goal distance
# need to subtract curent heading from the ball dir
# goal dir should be robot to goal not bloody ball to goal
# remove int touched ball
# something else here?
rotation, speed = net.activate(
[ball_dir, ball_dist, goal_dir,
int(robot_touched_ball)])
rotation = utils.clamp(rotation, -1.0, 1.0)
speed = utils.clamp(speed, -1.0, 1.0)
rotation *= 10 # rotation will be in degrees
speed *= 50 # max speed = 60
robot.angle += math.radians(rotation)
robot.velocity = (speed * math.cos(robot.angle - 1.5708),
speed * math.sin(robot.angle - 1.5708))
# step sim based on input
robot.angular_velocity = 0
robot.center_of_gravity = (10.5, 10.5)
space.step(1.0 / 60.0)
total_steps += 1
# session was ended from one of the callback listeners, so we know it's got the bonuses already
if reset_sim:
reset_sim = False
return fitness
# test failed to complete, still subtract total steps
return fitness - (total_steps / 1.5) + total_steps_touching_ball
def __init__(self):
# how can we uniquely identify them?
self._uuid = uuid.uuid4()
# NOTE using this would be cheating :)
# number of contacts they usually call
self.num_contacts = math.ceil(random.normalvariate(10, 3))
self.num_contacts = clamp(self.num_contacts, 1, 200)
# who do they usually call?
self.contacts = []
# how habitual are they in their contacts?
# 1 = always contacts the same set of contacts
# 0 = completely uniform random contact for each call
self.contact_regularity = random.normalvariate(.95, .01)
# how often do they call people?
# mean of 3 = 1/3 chance for average person to make call any given hour
self.cell_usage = random.normalvariate(3, .30)
if self.cell_usage < 0:
self.cell_usage = .001
# number of locations they usually call from
self.num_cells = math.ceil(random.normalvariate(10, 3))
self.num_cells = clamp(self.num_cells, 1, 200)
# what cells (locations) do they commonly call from?
self.cells = random.sample(range(num_cells), self.num_cells)
# how habitual are they in their locations?
# 1 = always uses the same set of locations
# 0 = completely uniform random cell for each call
self.cell_regularity = random.normalvariate(.8, .05)
# how frequently (on average) does someone change phones?
self.number_switch_frequency = 0
while self.number_switch_frequency < 24:
# stipulate that nobody change numbers more than once per day
frequency = 1.0 / change_numbers_every
self.number_switch_frequency = random.expovariate(frequency)
prob_per_hour = 1 / self.number_switch_frequency
# everybody "switches" at t=0
self.switch_times = [0]
for hour in range(end_time):
if random.random() < prob_per_hour:
self.switch_times.append(hour)
self.numbers = []
for time in self.switch_times:
self.numbers.append(CellNumber(self))
assert len(self.switch_times) == len(self.numbers)
def collision(self, pipe: "Pipe"):
closestX = clamp(self.position[0], pipe.position[0], pipe.position[0] + self.game.settings["pipeWidth"])
closestY = clamp(self.position[1], pipe.position[1], pipe.position[1] + pipe.height)
# Calculate the distance between the circle's center and this closest point
distanceX = self.position[0] - closestX
distanceY = self.position[1] - closestY
# If the distance is less than the circle's radius, an intersection occurs
distanceSquared = (distanceX * distanceX) + (distanceY * distanceY)
return distanceSquared < (self.game.settings["birdRadius"] * self.game.settings["birdRadius"])
def update_range(self, ranges):
if not self.enabled:
return
scan_points = scan_to_points(
ranges, self.right_most_ray_degrees, -self.degrees_per_ray)
self.last_obstacles = scan_points
# if there are no obstacles.
if len(scan_points) == 0:
if self.requested_speed > 0:
self.blocked = False
self.actual_steer = clamp(-pi/9, pi/9, self.requested_steer)
self.basic_controls.set_steer(self.actual_steer)
self.speed_control.set_speed(self.requested_speed)
self.last_path = []
else: # there are obstacles.
self.blocked_paths = []
for dev in [0,-1,1,-2,2,-3,3,-4,4,-6,6,-8,8]:
# when there's obstacles around, don't allow the
# heading control to turn sharply.
req_steer = clamp(-pi/12, pi/12, self.requested_steer)
steer = req_steer + dev*pi/9.0
if abs(steer) > self.max_steer:
continue
path_length = min(self.requested_distance, 5)
path_points = make_path(steer, 0.25, path_length)
left_path = [Point(x-1.0, y) for x,y in path_points]
right_path = [Point(x+1.0, y) for x,y in path_points]
left_index = path_near_points(left_path, scan_points, 0.5)
right_index = path_near_points(right_path, scan_points, 0.5)
if left_index or right_index:
self.blocked_paths += path_points
else: # path is clear
self.actual_steer = steer
if self.requested_speed > 0:
self.basic_controls.set_steer(self.actual_steer)
self.speed_control.set_speed(self.requested_speed)
self.blocked = False
self.last_path = path_points
break
else:
# no path found
self.blocked = True
self.last_path = []
if self.requested_speed > 0:
self.speed_control.stop()
def write_colour(pixel_colour, samples_per_pixel):
r = pixel_colour.x
g = pixel_colour.y
b = pixel_colour.z
scale = 1.0 / samples_per_pixel
r = math.sqrt(scale * r)
g = math.sqrt(scale * g)
b = math.sqrt(scale * b)
return "%s %s %s" % (256 * utils.clamp(r, 0.0, 0.999), 256 * utils.clamp(
g, 0.0, 0.999), 256 * utils.clamp(b, 0.0, 0.999))
def tick(self, dtime):
if not self.engine.overlayMgr.currentOverlay == "Game":
return
if self.playerObject == None:
self.playerObject = self.engine.entityMgr.playerObject
self.screenWidth = self.engine.gfxMgr.viewPort.getActualWidth()
self.screenHeight = self.engine.gfxMgr.viewPort.getActualHeight()
# ---------- Mouse Orientation ----------
deltaX = self.ms.X.abs
deltaY = self.ms.Y.abs
if not self.playerObject == None and not self.keyboard.isKeyDown(OIS.KC_LCONTROL):
if (utils.distSquared2D(self.ms.X.abs, self.ms.Y.abs, self.screenWidth/2, self.screenHeight/2) > 350):
self.playerObject.yawRate = -1*float(self.ms.X.abs) / (self.screenWidth/2) + 1
self.playerObject.pitchRate = -1*float(self.ms.Y.abs) / (self.screenHeight/2) + 1
else:
self.playerObject.yawRate = 0
self.playerObject.pitchRate = 0
#----------make selected ent respond to keyboard controls-----------------------------------
if self.toggle >= 0:
self.toggle = self.toggle - dtime
self.keyboard.capture()
if not self.playerObject == None:
# Speed Up
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_W):
self.toggle = self.toggleMax
self.playerObject.desiredSpeed = utils.clamp(self.playerObject.desiredSpeed + self.playerObject.acceleration, 0, self.playerObject.maxSpeed)
# Slow down
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_S):
self.toggle = self.toggleMax
self.playerObject.desiredSpeed = utils.clamp(self.playerObject.desiredSpeed - self.playerObject.acceleration, 0, self.playerObject.maxSpeed)
# Strafe Left.
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_A):
pass
# Strafe Right.
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_D):
pass
# Roll Left.
if self.keyboard.isKeyDown(OIS.KC_Q):
temp = ogre.Quaternion()
temp.FromAngleAxis(self.playerObject.turningRate*dtime, ogre.Vector3(0, 0, 1))
self.playerObject.orientation *= temp
# Roll Right.
if self.keyboard.isKeyDown(OIS.KC_E):
temp = ogre.Quaternion()
temp.FromAngleAxis(-self.playerObject.turningRate*dtime, ogre.Vector3(0, 0, 1))
self.playerObject.orientation *= temp
def tick(self, dtime):
# ---------- Mouse Orientation ----------
deltaX = self.ms.X.abs
deltaY = self.ms.Y.abs
if not self.playerObject == None:
if (utils.distSquared2D(self.ms.X.abs, self.ms.Y.abs, self.screenWidth/2, self.screenHeight/2) > 1000):
self.playerObject.yawRate = -1*float(self.ms.X.abs) / (self.screenWidth/2) + 1
self.playerObject.pitchRate = -1*float(self.ms.Y.abs) / (self.screenHeight/2) + 1
else:
self.playerObject.yawRate = 0
self.playerObject.pitchRate = 0
#----------make selected ent respond to keyboard controls-----------------------------------
if self.toggle >= 0:
self.toggle = self.toggle - dtime
if self.toggleFire >= 0:
self.toggleFire = self.toggleFire - dtime
if self.toggle < 0:
self.keyboard.capture()
if not self.playerObject == None:
# Speed Up
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_W):
self.toggle = self.toggleMax
self.playerObject.desiredSpeed = utils.clamp(self.playerObject.desiredSpeed + self.playerObject.acceleration, 0, self.playerObject.maxSpeed)
# Slow down
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_S):
self.toggle = self.toggleMax
self.playerObject.desiredSpeed = utils.clamp(self.playerObject.desiredSpeed - self.playerObject.acceleration, 0, self.playerObject.maxSpeed)
# Turn Left.
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_A):
pass
# Turn Right.
if self.toggle < 0 and self.keyboard.isKeyDown(OIS.KC_D):
pass
# Turn Up.
if self.keyboard.isKeyDown(OIS.KC_Q):
temp = ogre.Quaternion()
temp.FromAngleAxis(ogre.Degree(-0.5), ogre.Vector3(1, 0, 0))
self.playerObject.orientation *= temp
# Turn Down.
if self.keyboard.isKeyDown(OIS.KC_E):
temp = ogre.Quaternion()
temp.FromAngleAxis(ogre.Degree(0.5), ogre.Vector3(1, 0, 0))
self.playerObject.orientation *= temp
# Fire button
if self.keyboard.isKeyDown(OIS.KC_SPACE) and self.toggleFire < 0:
self.toggleFire = self.toggleFireMax
self.playerObject.fireMissile()
def update(self, char):
y, x = self.cursor_pos
if char == curses.KEY_RIGHT:
x += 1
elif char == curses.KEY_LEFT:
x -= 1
elif char == curses.KEY_UP:
y -= 1
elif char == curses.KEY_DOWN:
y += 1
elif char in (ord(' '), ord('z')):
self.mark(x, y)
elif char == ord('x'):
self.discard(x, y)
elif char == ord('s'):
self.save()
x = utils.clamp(x, 0, self.level.width - 1)
y = utils.clamp(y, 0, self.level.height - 1)
self.cursor_pos = (y, x)
# Update rows side.
types, groups = self.get_marks_row(y)
self.level.clear_row(y)
marks = 0
for i, group in enumerate(groups[-1::-1]):
if types[-(i + 1)] == self.MARK:
marks += 1
self.level.add_to_left(self.level.left_margin - marks, y, group)
# Update cols side.
types, groups = self.get_marks_col(x)
self.level.clear_col(x)
marks = 0
for i, group in enumerate(groups[-1::-1]):
if types[-(i + 1)] == self.MARK:
marks += 1
self.level.add_to_top(x, self.level.top_margin - marks, group)
def update(self):
current_speed = self.vstate.speed
dt = self.vstate.time - self.last_update_time
assert(current_speed >= 0)
if self.stopping and abs(current_speed) < 0.1:
self.stopping = False
if self.stopping or self.target_speed == 0:
integral = 0
error = 0
throttle = 0
brake = 2
else:
error = abs(self.target_speed) - current_speed
integral = self.last_integral + error*dt
derivative = (error - self.last_error)/dt
throttle = self.Kp*error + self.Ki*integral + self.Kd*derivative
throttle = clamp(0, 40, throttle)
brake = 0
if self.target_speed < 0:
throttle = -throttle
self.last_error = error
self.last_integral = integral
self.controls.throttle = throttle
self.controls.brake = brake
def scale(self, app_def, rule_manager):
""" Take scale action
:param app_def: dict of marathon application settings
:param rule_manager: object of scaling properties.
:return: marathon response
"""
if not app_def.is_app_participating:
return
scale_factor = int(rule_manager.last_triggered_criteria.get("scale_factor"))
min_instances = int(rule_manager.min_instances)
max_instances = int(rule_manager.max_instances)
scale_to = app_def.instances + scale_factor
scale_to_size = clamp(scale_to, min_instances, max_instances)
if app_def.instances == scale_to_size:
msg = "{app_name}: application already scaled to {size}"
self.logger.info(msg.format(app_name=app_def.app_name,
size=scale_to_size))
return
self.marathon_client.scale_app(app_def.id, scale_to_size)
msg = "{app_name}: scaled to {size}"
self.logger.info(msg.format(app_name=app_def.app_name,
size=scale_to_size))
def short_tags(self):
s = self.page_html
s = s.replace('{{sn}}', config.site_name) # site name
s = s.replace('{{st}}', config.site_tag) # site tag
# galleries
gallery_re = re.compile('\{\{gallery=".*"\}\}')
for gal in gallery_re.findall(s):
gallery_address = gal.split('"')[1]
s = s.replace(gal, gallery.make_gallery(gallery_address))
# social networks
s = s.replace('{{social_small}}', pages.social_small())
s = s.replace('{{social_large}}', pages.social_large())
s = s.replace('{{disable comments}}', '')
s = s.replace('{{disable sidebar}}', '')
s = s.replace('{{hidden}}', '')
s = s.replace('{{ctime}}', time.ctime(time.time()))
# In order to make the website as portable as possible,
# all links generated are relative to the root
# rli = relative link index
rli = utils.clamp(0, self.level, self.level - 1)
s = s.replace('{{wr}}', '../'*rli)
self.page_html = s
def on_mouse_motion(self, x, y, dx, dy):
"""Change the rotation of the player when the mouse moves.
"""
x, y = self.rotation
x, y = x + dx * self.rotation_speed, y + dy * self.rotation_speed
y = clamp(-90, y, 90)
self.rotation = (x, y)
def menu_pos(self, value):
oldpos = self._menu_pos
newpos = clamp(value, 0, settings.LCD_ROWS - 1)
self._menu_pos = newpos
if oldpos != newpos:
self.notify_observers()
"""self.lcd.cursor_pos = (oldpos, 0)
def texture(data, size=[3,3,3], method='drange', mask=None, th=0, bins=256):
data_clamped, bins = clamp(data, th=th, mask=mask, bins=bins)
hist = np.zeros(bins)
_histogram(hist, data_clamped.flat)
if method in texture_measures:
return _texture(data_clamped, hist, size, texture_measures[method])
else:
return _texture(data_clamped, hist, size, texture_measures['custom'], method)
def zoom(cont):
global zoom_level, target_altitude
own = cont.owner
#zoom_level = own["zoom_level"]
mouse_w_up = own.sensors['MouseWUp']
mouse_w_down = own.sensors['MouseWDown']
if mouse_w_up.positive:
zoom_level = clamp(zoom_level-1, 0, 7)
target_altitude = zoom_to_altitude(zoom_level)
elif mouse_w_down.positive:
zoom_level = clamp(zoom_level+1, 0, 7)
target_altitude = zoom_to_altitude(zoom_level)
own.worldPosition.z = lerp(own.worldPosition.z, target_altitude, .1)
def tick(self, dtime):
GenericShip.tick(self, dtime)
self.energy = utils.clamp(self.energy + self.energyRechargeRate*dtime, 0, self.maxEnergy)
if self.energy < self.fireEnergyCost:
self.canFire = False
else:
self.canFire = True
def faceDetected(points):
'''
This is the callback function that is called when ever a ball is detected!
It performs the PD controller task required to track the ball
Inputs => geometry_msgs.points==> points is a pair of (x,y) that is the top_left corner of the tracked ball
output:
It directly writes to the angles dictionary that is a dictionary of list of pan and tilt angles indepedently
It Performs PD controller on the pan and tilt command then append it to the dictionary so that any function
interested can use them to publish the values.
'''
global tPrev
tCurrent=time.time()
dt=abs(tCurrent-tPrev)
# grab the top left corner as x-axis
x=points.x
# gradthe top left corner as y-axis
y=points.y
# Ideally the point should be at the center of the camera view (i.e (x,y)= (0.5,0.5))
ideal_left = 0.5#.3
ideal_top = 0.45# .45
# Find the derivative terms
dx=(x-x_prev)/dt
dy=(y-y_prev)/dt
# Find the control signal using PD controller
correction_pan = kx*(ideal_left - x)-kdx*(dx)
correction_tilt= ky*(ideal_top - y)-kdy*(dy)
# prepare for clamping the values
temp1=board["angles"]["pan"][-1] + correction_pan
temp2=board["angles"]["tilt"][-1]- correction_tilt
# #rospy.loginfo("This is pan and tilt correction %f %f : ",correction_pan, correction_tilt)
# Clamp pan and tilt angles
board["angles"]["pan"].append(90-clamp(0, temp1, 90))#
board["angles"]["tilt"].append(90-clamp(0, temp2, 90))#90-
tPrev=tCurrent
def set_steer(self, angle):
angle = clamp(-self.max_steer, self.max_steer, angle)
# allow manual external steering control in reverse.
if not self.enabled or self.requested_speed < 0:
self.basic_controls.set_steer(angle)
self.requested_steer = angle
self.actual_steer = angle
else:
self.requested_steer = angle
def menu_pos(self, value):
oldpos = self._menu_pos
newpos = clamp(value, 0, LCD_ROWS - 1)
self._menu_pos = newpos
if oldpos != newpos:
self.lcd.cursor_pos = (oldpos, 0)
self.lcd.write_string(self.menu_prefix)
self.lcd.cursor_pos = (newpos, 0)
self.lcd.write_string(self.menu_selected)
def tick(self, dtime):
#----------position-----------------------------------
timeScaledAcceleration = self.ent.acceleration * dtime
self.ent.speed += utils.clamp( self.ent.desiredSpeed - self.ent.speed, -timeScaledAcceleration, timeScaledAcceleration)
self.ent.vel.x = math.cos(-self.ent.heading) * self.ent.speed
self.ent.vel.z = math.sin(-self.ent.heading) * self.ent.speed
self.ent.vel.y = 0
self.ent.pos = self.ent.pos + (self.ent.vel * dtime)
#------------heading----------------------------------
timeScaledRotation = self.ent.turningRate * dtime
angleDiff = utils.diffAngle(self.ent.desiredHeading, self.ent.heading)
dheading = utils.clamp(angleDiff, -timeScaledRotation, timeScaledRotation)
self.ent.heading += dheading
def update(self, all_mines):
'''
This is the real brains function. Takes an iterable of mines. It first
takes sensor readings and feed these to the ANN of our minesweeper.
The inputs are:
1) A vector (Vector2D) to the closest mine,
2) The "look at" vector (also a Vector2D).
The brain(ANN) returns 2 outputs, ltrack and rtrack - which are forces
applied on left and right tracks, respectively. Depending on these, the
acceleration and/or the rotation is calculated and the position vector
is updated accordingly.
'''
# Inputs to the brain.
inputs = []
# First input: vector to the closest mine.
closest_mine = self.get_closest_mine(all_mines)
closest_mine.normalize()
# Place the inputs on the input list
inputs.append(closest_mine.x)
inputs.append(closest_mine.y)
inputs.append(self.look_at.x)
inputs.append(self.look_at.y)
# Now, excite the brain and get the feedback
output = self.brain.excite(inputs, settings.BIAS, filter_sigmoid=True)
# Make sure we get back the expected number of outputs
if len(output) != settings.NUM_OUTPUTS:
raise Exception( 'An error occurred: The number of outputs from '
+'the ANN is not what was expected.')
self.ltrack, self.rtrack = output
rot_force = self.ltrack - self.rtrack
rot_force = clamp(rot_force,
-settings.MAX_TURN_RATE,
settings.MAX_TURN_RATE)
# New rotation and speed:
self.rotation += rot_force
self.speed = self.ltrack + self.rtrack
# Get the new look at:
self.look_at.x = -math.sin(self.rotation)
self.look_at.y = math.cos(self.rotation)
# Get the new position:
self.position += (self.look_at * self.speed)
# Wrap around the screen
if self.position.x > settings.WINDOW_WIDTH:
self.position.x = 0
if self.position.x < 0:
self.position.x = settings.WINDOW_WIDTH
if self.position.y > settings.WINDOW_HEIGHT:
self.position.y = 0
if self.position.y < 0:
self.position.y = settings.WINDOW_HEIGHT
def faceDetected(boxes):
face_left = boxes.faces[0].top_left.x
face_top = boxes.faces[0].top_left.y
face_width = boxes.faces[0].width_height.x
face_height = boxes.faces[0].width_height.y
# Calculate correction
ideal_left = 0.5 - face_width / 2.0
correction_pan = (ideal_left - face_left) * 2.5
ideal_top = 0.5 - face_height / 2.0
correction_tilt = (ideal_top - face_top) * 2.5
temp1=angles["pan"][-1] + correction_pan
temp2=angles["tilt"][-1]- correction_tilt
# Clamp pan and tilt angles
board["angles"]["pan"].append(clamp(0, temp1, 90))
board["angles"]["tilt"].append(clamp(0, temp2, 90))
def pan(cont):
global edge_time, momentum
own = cont.owner
#mouse_sens = own.sensors['Mouse']
#mouse = Vector(mouse_sens.position)
mouse = Vector(bge.logic.mouse.position)
contact = False
if (mouse.x >= .95):
contact = True
edge_time += 1
elif (mouse.x <= .05):
contact = True
edge_time += 1
if (mouse.y >= .95):
contact = True
edge_time += 1
elif (mouse.y <= .05):
edge_time += 1
contact = True
if contact:
target_momentum = (mouse - Vector((.5,.5))) * get_pan_speed(edge_time)
target_momentum.y *= -1
momentum = momentum.lerp(target_momentum, .1)
else:
edge_time = 0
momentum *= damping
own.worldPosition.x = clamp(own.worldPosition.x + momentum.x, -128, 128)
own.worldPosition.y = clamp(own.worldPosition.y + momentum.y, -158, 108)
def update(self, step_time):
def change_dir_vel(entities, direction, velocity):
for entity in entities:
entity.body.direction = direction
entity.body.set_velocity(velocity)
if(self.moving_left or self.moving_right):
if self.moving_left:
change_dir_vel(self.paddles, Vector2(-1,0), PADDLE_VELOCITY)
if self.game_status == GameLayer.INITIALIZATION:
change_dir_vel(self.balls, Vector2(-1,0), PADDLE_VELOCITY)
else:
change_dir_vel(self.paddles, Vector2(1,0), PADDLE_VELOCITY)
if self.game_status == GameLayer.INITIALIZATION:
change_dir_vel(self.balls, Vector2(1,0), PADDLE_VELOCITY)
else:
change_dir_vel(self.paddles, ZERO2, magnitude(ZERO2))
if self.game_status == GameLayer.INITIALIZATION:
change_dir_vel(self.balls, ZERO2, magnitude(ZERO2))
if self.push_balls and self.game_status == GameLayer.INITIALIZATION:
for ball in self.balls:
if ball.body.is_static:
ball.body.is_static = False
v = Vector2(BALL_VELOCITY_X, BALL_VELOCITY_Y)
change_dir_vel(self.balls, normalize(v), magnitude(v))
self.push_balls = False
self.game_status = GameLayer.GAME_LOOP
# Remove bricks that have been destroyed
free_brick_list = []
for brick in self.bricks:
if brick.health_points == 0:
self.unregister_entity(brick)
free_brick_list.append(brick)
self.bricks = [ b for b in self.bricks if free_brick_list.count(b) == 0 ]
for paddle in self.paddles:
# Integrate paddle
paddle.body.rect.position = sum(paddle.body.rect.position,
mul(paddle.body.direction, paddle.body.velocity * step_time))
# Relocate paddle position to a valid position range
paddle.body.rect.position.x = utils.clamp(paddle.body.rect.position.x, 0,
WINDOW_WIDTH - PADDLE_WIDTH)
paddle.body.rect.position.y = WINDOW_HEIGHT - PADDLE_HEIGHT - PADDLE_LINE_SPACING
for ball in self.balls:
if ball.body.is_static:
pos_r = Vector2((PADDLE_WIDTH - BALL_WIDTH) * 0.5, - BALL_HEIGHT)
ball.body.rect.position = sum(self.paddles[0].body.rect.position, pos_r)
def tick(self, dtime):
''' Updates Position, Yaw, Pitch, and Roll. '''
# Position ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ #
timeScaledAcceleration = self.ent.acceleration * dtime
self.ent.speed += utils.clamp( self.ent.desiredSpeed - self.ent.speed, -timeScaledAcceleration, timeScaledAcceleration)
self.ent.vel.y = self.ent.speed * math.sin( math.radians( self.ent.pitch ) )
## xz = self.ent.speed * math.cos( math.radians( self.ent.pitch ) )
self.ent.vel.x = self.ent.speed * math.cos( math.radians( self.ent.yaw ) )
self.ent.vel.z = self.ent.speed * -math.sin( math.radians( self.ent.yaw ) )
self.ent.pos = self.ent.pos + ( self.ent.vel * dtime )
# Yaw ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ #
timeScaledYaw = self.ent.yawRate * dtime
angleDiff = utils.diffAngle(self.ent.desiredYaw, self.ent.yaw)
self.ent.deltaYaw = utils.clamp(angleDiff, -timeScaledYaw, timeScaledYaw)
## print '---' + self.ent.uiname + '---'
## print "angleDiff: %f, timeScaledYaw: %f, deltaYaw: %f " % (angleDiff, timeScaledYaw, self.ent.deltaYaw )
## print "yaw: %f, desiredYaw: %f, yawRate: %f" % (self.ent.yaw, self.ent.desiredYaw, self.ent.yawRate)
self.ent.yaw = utils.fixAngle( self.ent.yaw + self.ent.deltaYaw)
# Pitch ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ #
timeScaledPitch = self.ent.pitchRate * dtime
angleDiff = utils.diffAngle(self.ent.desiredPitch, self.ent.pitch)
self.ent.deltaPitch = utils.clamp(angleDiff, -timeScaledPitch, timeScaledPitch)
## print "angleDiff: %f, timeScaledPitch: %f, deltaPitch: %f " % (angleDiff, timeScaledPitch, self.ent.deltaPitch )
## print "pitch: %f, desiredPitch: %f, pitchRate: %f" % (self.ent.pitch, self.ent.desiredPitch, self.ent.pitchRate)
self.ent.pitch = utils.fixAngle( self.ent.pitch + self.ent.deltaPitch)
# Roll ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ #
if dtime > 0.001:
self.ent.desiredRoll = -self.ent.deltaYaw / dtime * 0.75
timeScaledRoll = self.ent.rollRate * dtime
angleDiff = utils.diffAngle(self.ent.desiredRoll, self.ent.roll)
self.ent.deltaRoll = utils.clamp(angleDiff, -timeScaledRoll, timeScaledRoll)
## print "angleDiff: %f, timeScaledRoll: %f, dRoll: %f " % (angleDiff, timeScaledRoll, self.ent.deltaRoll )
## print "roll: %f, desiredRoll: %f, rollRate: %f" % (self.ent.roll, self.ent.desiredRoll, self.ent.rollRate)
## print "---"
self.ent.roll = utils.fixAngle( self.ent.roll + self.ent.deltaRoll)
# Physics -------------------------------------------------------------------- #
def tick(self, dtime):
#----------position-----------------------------------
timeScaledAcceleration = self.ent.acceleration * (dtime * 100)
self.ent.speed += utils.clamp( self.ent.desiredSpeed - self.ent.speed, -timeScaledAcceleration, timeScaledAcceleration)
#self.ent.vel.x = math.cos(-self.ent.heading) * self.ent.speed
#self.ent.vel.z = math.sin(-self.ent.heading) * self.ent.speed
#self.ent.vel.y = 0
self.ent.vel = Vector3(self.ent.speed * math.cos(-self.ent.heading), 0, self.ent.speed * math.sin(-self.ent.heading))
self.ent.pos = self.ent.pos + (self.ent.vel * dtime)
#------------heading----------------------------------
timeScaledRotation = self.ent.turningRate * (dtime*20)
angleDiff = utils.diffAngle(self.ent.desiredHeading, self.ent.heading)
dheading = utils.clamp(angleDiff, -timeScaledRotation, timeScaledRotation)
print self.ent.desiredHeading
print "meow"
print self.ent.heading
self.ent.heading += dheading
