def run(self):
"""Spin until we see at least two beacons
While at the same time turning Cheesy's head
"""
# Do we see at least two beacons?
global visible_beacons
seen_beacons = tuple(visible_beacons())
if sum(seen_beacons) >= 2:
self.finish()
# Turn head in the other direction
if self.getTime >= 3.0:
self.resetTime()
if Spin.direction == Spin.LEFT:
Spin.direction = Spin.RIGHT
else:
Spin.direction = Spin.LEFT
# Look left or right
commands.setHeadPan(Spin.direction, 1)
memory.speech.say('Spinning!')
commands.setWalkVelocity(0, 0, 0.3)
def run(self):
dt = 1.0/30.0
self.calc_integral(dt)
bearing_ball = self.ball.visionBearing
bearing_goal = self.goal.visionBearing
delta_bearing = bearing_ball - bearing_goal
distance = self.ball.visionDistance
elevation = core.RAD_T_DEG * self.ball.visionElevation
commands.setHeadPanTilt(bearing_ball,self.pan,1.5)
if dt == 0:
theta_cont = self.k_t[0] * bearing_ball + self.k_t[1] * self.theta_integral_ball
dist_cont = self.k_d[0] * (distance - self.dist) + self.k_d[1] * self.dist_integral
y_cont = self.k_y[0] * delta_bearing + self.k_y[1] * self.theta_integral_delta
delta_derivative = 0.0
else:
theta_cont = self.k_t[0] * bearing_ball + self.k_t[1] * self.theta_integral_ball + self.k_t[2] *(bearing_ball - self.theta_prev_ball) / dt
dist_cont = self.k_d[0] * (distance - self.dist) + self.k_d[1] * self.dist_integral + self.k_d[2] *(distance - self.dist_prev) / dt
y_cont = self.k_y[0] * delta_bearing + self.k_y[1] * self.theta_integral_delta + self.k_y[2] *(delta_bearing - self.theta_prev_delta) / dt
if ((bearing_ball - self.theta_prev_ball)/dt) > 20.0:
theta_cont = 0.0
dist_cont = 0.0
if ((delta_bearing - self.theta_prev_delta)/dt) > 20.0:
y_cont = 0.0
delta_derivative = (delta_bearing - self.theta_prev_delta) / dt
commands.setWalkVelocity(dist_cont, y_cont, theta_cont)
self.theta_prev_ball = bearing_ball
self.theta_prev_delta = delta_bearing
self.dist_prev = distance
def run(self):
vel_turn_gain = 1.00
vel_y_gain = 1.00
vel_y = 0.0
vel_x = 0.30
vel_turn = 0.05
ball = memory.world_objects.getObjPtr(core.WO_BALL)
goal = memory.world_objects.getObjPtr(core.WO_OPP_GOAL)
x_desired = 360.0 / 2
if ball.seen:
vel_y += vel_y_gain * (x_desired - ball.imageCenterX) / (x_desired)
vel_x = 0.4
else:
vel_x = -0.45
# TODO what if the goal is not seen???
if goal.seen:
vel_turn += vel_turn_gain * (x_desired - goal.imageCenterX) / (x_desired)
if goal.visionDistance < 2300:
self.finish()
commands.setWalkVelocity(vel_x, vel_y, vel_turn)
def run(self):
ball = world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
print "Ball Distance: ", ball.visionDistance
self.last_seen = 0
ball_x, ball_y = self.get_object_position(ball)
if ball_x < 150 and abs(ball_y - self.kick_offset) < 40:
self.finishCt += 1
else:
self.finishCt = max(self.finishCt - 1, 0)
xVel = self.x.update(ball_x) / 300.0
yVel = self.y.update(ball_y - self.kick_offset) / 200.0
xVel = max(min(xVel,0.5),-0.5)
yVel = max(min(yVel,1.0),-1.0)
print "Error: {0}, {1} --> Walk Velocity: {2}, {3}".format(ball_x, ball_y - self.kick_offset, xVel, yVel)
commands.setWalkVelocity(xVel, yVel, 0.0)
else:
self.last_seen += 1
if self.last_seen > 10:
commands.setWalkVelocity(0, 0, 0)
self.finishCt = max(self.finishCt - 1, 0)
if self.finishCt > 3:
self.finish()
def run(self):
ball = memory.world_objects.getObjPtr(core.WO_BALL)
maxYaw = 1.0
minYaw = -1.0
# print(core.joint_values[core.HeadYaw])
if ball.seen:
print("Elevation ",ball.visionElevation,", Bearing ",ball.visionBearing,", Distance ",ball.visionDistance)
# commands.setHeadPan(0,0.2)
# return ball.visionElevation, ball.visionBearing,ball.visionDistance
elif not ball.seen and core.joint_values[core.HeadYaw] <=maxYaw and self.yawFlag and not self.turnFlag:
#print("turn+")
commands.setHeadPan(core.joint_values[core.HeadYaw]+0.07,0.01)
if core.joint_values[core.HeadYaw]+0.05 >= maxYaw:
self.yawFlag=False
elif not ball.seen and core.joint_values[core.HeadYaw] >=minYaw and not self.yawFlag and not self.turnFlag:
#print("turn-")
commands.setHeadPan(core.joint_values[core.HeadYaw]-0.07,0.01)
if core.joint_values[core.HeadYaw]-0.05 <= minYaw:
self.yawFlag=True
self.turnFlag=True
elif not ball.seen and self.turnFlag:
commands.setHeadPan(0,0.1)
commands.setWalkVelocity(0, 0, 0.4)
self.turnFlag = False
def run(self):
print("Start walk")
ball = memory.world_objects.getObjPtr(core.WO_BALL)
goal_dist = 200
goal_theta = 0
max_speed = 1.0
dt = 1e-2
print("Elevation ", ball.visionElevation, ", Bearing ",
ball.visionBearing, ", Distance ", ball.visionDistance)
err_dist2 = ball.visionDistance - goal_dist
err_theta2 = ball.visionBearing - goal_theta
K_P = 5e-4
K_Pt = 1
K_I = 5e-5
K_It = 1e-3
K_D = 1e-6
K_Dt = 0.0
self.err_d_run += err_dist2 * dt
self.err_t_run += err_theta2 * dt
err_d_delta = (err_dist2 - self.err_dist) / dt
err_t_delta = (err_theta2 - self.err_theta) / dt
self.err_dist = err_dist2
self.err_theta = err_theta2
x_dot = K_P * self.err_dist + K_I * self.err_d_run + K_D * err_d_delta
t_dot = K_Pt * self.err_theta + K_It * self.err_t_run + K_Dt * err_t_delta
print("Output velocity ", x_dot)
print("Output angular velocity ", t_dot)
commands.setWalkVelocity(min(x_dot, max_speed), 0,
max(min(t_dot, max_speed), -max_speed))
if self.err_dist <= 0 or (ball.visionDistance < 500
and abs(x_dot) < 0.08):
print("Done at ", ball.imageCenterX, ", ", ball.imageCenterY)
commands.setWalkVelocity(0, 0, 0)
self.finish()
def run(self):
ball = world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
print "Ball Distance: ", ball.visionDistance
self.last_seen = 0
ball_x, ball_y = self.get_object_position(ball)
if ball_x < 160 and abs(ball_y - self.kick_offset) < 30:
self.finishCt += 1
else:
self.finishCt = max(self.finishCt - 1, 0)
xVel = self.x.update(ball_x)
yVel = self.y.update(ball_y - self.kick_offset)
tVel = (ball.visionBearing - self.initialTheta) / 0.87
xVel = max(min(xVel, 0.5), -0.5)
yVel = max(min(yVel, 1.0), -1.0)
tVel = 0 #max(min(tVel,1.0),-1.0)
if tVel * yVel > 0:
tVel = tVel * -1
print "Final: {0}, {1}, {2} --> Walk Velocity: {3}, {4}, {5}".format(
ball_x, ball_y - self.kick_offset,
ball.visionBearing - self.initialTheta, xVel, yVel, tVel)
commands.setWalkVelocity(xVel, yVel, tVel)
else:
self.last_seen += 1
if self.last_seen > 10:
commands.setWalkVelocity(0, 0, 0)
self.finishCt = max(self.finishCt - 1, 0)
if self.finishCt > 3:
self.x.reset()
self.y.reset()
self.finish()
def run(self):
num_beacons_seen = 0
beac = -1 # no beacons are seen
start_time = self.getTime()
commands.setHeadPanTilt(pan=0, tilt=0, time=0.1, isChange=True)
seen_beacon = None
for beacon_name in BEACONS:
beacon = mem_objects.world_objects[beacon_name]
if beacon.seen:
seen_beacon = beacon
if not seen_beacon:
commands.setWalkVelocity(0, 0, 0)
self.postSignal('no_towards_beacon')
else:
dist = seen_beacon.visionDistance
bearing = seen_beacon.visionBearing
print("beacon beating: ", bearing, "beacon distance", dist)
if dist < STOP_DISTANCE:
commands.setWalkVelocity(0, 0, 0)
self.postSignal('stop_towards_beacon')
else:
global vx, vy, vtheta
vx = 1
vy = 0
# vtheta = 0
vtheta = bearing
# commands.setWalkVelocity(0.5,0,bearing)
self.postSignal('keep_towards_beacon')
def run(self):
ball = world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
print "Ball Distance: ", ball.visionDistance
self.last_seen = 0
ball_x, ball_y = self.get_object_position(ball)
bearing = ball.visionBearing;
if ball_x < 300 and abs(bearing) < 10 * core.DEG_T_RAD:
self.finishCt += 1
else:
self.finishCt = max(self.finishCt - 1, 0)
xVel = self.x_pid.update(ball_x)
yVel = 0 #self.y.update(ball_y - self.kick_offset) / 200.0
tVel = self.t_pid.update(bearing)
xVel = max(min(xVel,0.5),-0.5)
yVel = max(min(yVel,1.0),-1.0)
tVel = max(min(tVel,1.0),-1.0)
print "Approach: {0}, {1} --> Walk Velocity: {2}, {3}".format(ball_x, bearing, xVel, tVel)
commands.setWalkVelocity(xVel, yVel, tVel)
else:
self.last_seen += 1
if self.last_seen > 10:
print "Search!"
commands.setWalkVelocity(0, 0, 0.4)
self.finishCt = max(self.finishCt - 1, 0)
if self.finishCt > 3:
self.x_pid.reset()
self.t_pid.reset()
self.finish()
def run(self):
ball = world_objects.getObjPtr(core.WO_BALL)
goal = world_objects.getObjPtr(core.WO_OWN_GOAL)
selfRobot = world_objects.getObjPtr(robot_state.WO_SELF)
if ball.seen and goal.seen and abs(
goal.visionBearing) < 10 * core.DEG_T_RAD:
self.finishCt += 1
else:
self.finishCt = max(0, self.finishCt - 1)
tVel = max(min(1.0, ball.visionBearing / 0.87), -1.0)
xVel = max(min(1.0, 0.4 * ball.visionDistance / 300), -1.0)
if ball.visionDistance > 2000:
xVel = 0
print "Ball: {0} {1}".format(ball.visionDistance,
ball.visionBearing * core.RAD_T_DEG)
print "Rotate Velocity: {0} {1} Finish: {2}".format(
xVel, tVel, self.finishCt)
commands.setWalkVelocity(xVel, self.direction * 0.3, tVel)
if self.finishCt > 3:
if selfRobot.loc.y > 0:
self.direction = -1
else:
self.direction = 1
self.finish()
if abs(selfRobot.orientation) > 165 * core.DEG_T_RAD:
self.finishCt += 1
def run(self):
ball = world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
print "Ball Distance: ", ball.visionDistance
self.last_seen = 0
ball_x, ball_y = self.get_object_position(ball)
bearing = ball.visionBearing
if ball_x < 300 and abs(bearing) < 10 * core.DEG_T_RAD:
self.finishCt += 1
else:
self.finishCt = max(self.finishCt - 1, 0)
xVel = self.x_pid.update(ball_x)
yVel = 0 #self.y.update(ball_y - self.kick_offset) / 200.0
tVel = self.t_pid.update(bearing)
xVel = max(min(xVel, 0.5), -0.5)
yVel = max(min(yVel, 1.0), -1.0)
tVel = max(min(tVel, 1.0), -1.0)
print "Approach: {0}, {1} --> Walk Velocity: {2}, {3}".format(
ball_x, bearing, xVel, tVel)
commands.setWalkVelocity(xVel, yVel, tVel)
else:
self.last_seen += 1
if self.last_seen > 10:
print "Search!"
commands.setWalkVelocity(0, 0, 0.4)
self.finishCt = max(self.finishCt - 1, 0)
if self.finishCt > 3:
self.x_pid.reset()
self.t_pid.reset()
self.finish()
def run(self):
# print("Start walk")
ball = memory.world_objects.getObjPtr(core.WO_BALL)
goal_dist = 200
goal_theta = 0
max_speed = 1.0
dt = 1e-2
# print("Elevation ",ball.visionElevation,", Bearing ",ball.visionBearing,", Distance ",ball.visionDistance)
err_dist2 = ball.visionDistance - goal_dist
err_theta2 = ball.visionBearing - goal_theta
K_P = 7e-4
K_Pt = 1
K_I = 5e-4
K_It = 1e-3
K_D = 0.0
K_Dt = 0.0
self.err_d_run += err_dist2 * dt
self.err_t_run += err_theta2 * dt
err_d_delta = (err_dist2 - self.err_dist) / dt
err_t_delta = (err_theta2 - self.err_theta) / dt
self.err_dist = err_dist2
self.err_theta = err_theta2
x_dot = K_P * self.err_dist + K_I * self.err_d_run + K_D * err_d_delta
t_dot = K_Pt * self.err_theta + K_It * self.err_t_run + K_Dt * err_t_delta
# print("Output velocity ",x_dot)
# print("Output angular velocity ",t_dot)
commands.setWalkVelocity(min(x_dot, max_speed), 0,
max(min(t_dot, max_speed), -max_speed))
def run(self):
commands.setWalkVelocity(0.0, 0.0, 0.0)
commands.setHeadPanTilt(core.DEG_T_RAD * self.pan, self.tilt,
self.time)
commands.setHeadTilt(self.tilt)
if self.getTime() > (self.time + 0.3):
self.finish()
def run(self):
ball = world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
print "Ball Distance: ", ball.visionDistance
self.last_seen = 0
ball_x, ball_y = self.get_object_position(ball)
if ball_x < 160 and abs(ball_y - self.kick_offset) < 30:
self.finishCt += 1
else:
self.finishCt = max(self.finishCt - 1, 0)
xVel = self.x.update(ball_x)
yVel = self.y.update(ball_y - self.kick_offset)
tVel = (ball.visionBearing - self.initialTheta) / 0.87
xVel = max(min(xVel,0.5),-0.5)
yVel = max(min(yVel,1.0),-1.0)
tVel = 0 #max(min(tVel,1.0),-1.0)
if tVel * yVel > 0:
tVel = tVel * -1
print "Final: {0}, {1}, {2} --> Walk Velocity: {3}, {4}, {5}".format(ball_x, ball_y - self.kick_offset, ball.visionBearing - self.initialTheta, xVel, yVel, tVel)
commands.setWalkVelocity(xVel, yVel, tVel)
else:
self.last_seen += 1
if self.last_seen > 10:
commands.setWalkVelocity(0, 0, 0)
self.finishCt = max(self.finishCt - 1, 0)
if self.finishCt > 3:
self.x.reset()
self.y.reset()
self.finish()
def run(self):
robot = core.world_objects.getObjPtr(core.robot_state.WO_SELF)
print "** Current Location X: %d Y: %d" % (robot.loc.x, robot.loc.y)
print "** Confidence: " + str(robot.radius)
print "** Distance: " + str(robot.visionDistance)
print "** Bearing: " + str(robot.visionBearing)
print "** Orientation: " + str(robot.orientation)
print "** RADIUS: " + str(robot.radius)
commands.stand()
if self.getTime() > 4:
if (robot.radius < 0.40 and robot.radius > 0.0):
self.postSignal(Choices.Stand)
elif (robot.radius < 0.65 and robot.radius > 0.0):
commands.stand()
elif robot.visionDistance > DTHRESHOLD:
print "_______Distance __"
value = min(0.001 * robot.visionDistance, 0.30)
commands.setWalkVelocity(value, 0.0, kp*robot.visionBearing)
else:
commands.stand()
print "_____Localization Complete______ " + str(robot.visionDistance)
#core.speech.say("Localization Complete")
self.postSignal(Choices.Stand)
if (self.getTime() > 9 and robot.visionDistance < ATHRESHOLD and robot.visionDistance > DTHRESHOLD):
core.speech.say("Approach")
self.postSignal(Choices.Stand)
def run(self):
"""Spin until we see at least two beacons
While at the same time turning Cheesy's head
"""
# Do we see at least two beacons?
global visible_beacons
seen_beacons = tuple(visible_beacons())
if sum(seen_beacons) >= 2:
self.finish()
# Turn head in the other direction
if self.getTime >= 3.0:
self.resetTime()
if Spin.direction == Spin.LEFT:
Spin.direction = Spin.RIGHT
else:
Spin.direction = Spin.LEFT
# Look left or right
commands.setHeadPan(Spin.direction, 1)
memory.speech.say("Spinning!")
commands.setWalkVelocity(0, 0, 0.3)
def run(self):
dt = 1.0 / 30.0
self.calc_integral(dt)
err_bearing = self.ball.bearing - self.bearing
distance = self.ball.distance
elevation = core.RAD_T_DEG * self.ball.visionElevation
commands.setHeadPanTilt(0.0, -45.0, 1.5)
if dt == 0:
dist_cont = self.k_d[0] * (
distance - self.dist) + self.k_d[1] * self.dist_integral
y_cont = self.k_y[0] * err_bearing + self.k_y[
1] * self.theta_integral_goal
delta_derivative = 0.0
else:
dist_cont = self.k_d[0] * (distance - self.dist) + self.k_d[
1] * self.dist_integral + self.k_d[2] * (distance -
self.dist_prev) / dt
y_cont = self.k_y[0] * err_bearing + self.k_y[
1] * self.theta_integral_ball + self.k_y[2] * (
err_bearing - self.theta_prev_ball) / dt
if ((err_bearing - self.theta_prev_ball) / dt) > 20.0:
dist_cont = 0.0
if ((err_bearing - self.theta_prev_ball) / dt) > 20.0:
y_cont = 0.0
delta_derivative = (err_bearing - self.theta_prev_ball) / dt
commands.setWalkVelocity(dist_cont, y_cont, 0.0)
self.theta_prev_ball = err_bearing
self.dist_prev = distance
def run(self):
"""Keep spinning until you see the ball"""
ball = memory.world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
self.finish()
commands.setWalkVelocity(0, 0, -0.25)
def run(self):
ball = world_objects.getObjPtr(core.WO_BALL)
goal = world_objects.getObjPtr(core.WO_OWN_GOAL)
selfRobot = world_objects.getObjPtr(robot_state.WO_SELF)
if ball.seen and goal.seen and abs(goal.visionBearing) < 10 * core.DEG_T_RAD:
self.finishCt += 1
else:
self.finishCt = max(0,self.finishCt - 1)
tVel = max(min(1.0,ball.visionBearing / 0.87),-1.0)
xVel = max(min(1.0, 0.4 * ball.visionDistance / 300), -1.0)
if ball.visionDistance > 2000:
xVel = 0
print "Ball: {0} {1}".format(ball.visionDistance,ball.visionBearing * core.RAD_T_DEG)
print "Rotate Velocity: {0} {1} Finish: {2}".format(xVel,tVel,self.finishCt)
commands.setWalkVelocity(xVel,self.direction * 0.3,tVel)
if self.finishCt > 3:
if selfRobot.loc.y > 0:
self.direction = -1
else:
self.direction = 1
self.finish()
if abs(selfRobot.orientation) > 165 * core.DEG_T_RAD:
self.finishCt += 1
def run(self):
commands.stand()
commands.setWalkVelocity(0, 0, 0.05)
if self.getTime() > 5.0:
commands.setWalkVelocity(0, 0, 0)
memory.speech.say("Starting localization")
self.finish()
def run(self):
commands.setHeadTilt(
-10) # Tilt head up so we can see goal (default = -22)
commands.setWalkVelocity(0.4, 0, 0)
if self.getTime() > 15.0:
self.finish()
def run(self):
"""Keep spinning until you see the ball"""
ball = memory.world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
self.finish()
commands.setWalkVelocity(0, 0, -0.25)
def move_to_position(self, target_x, target_y):
robot = world_objects.getObjPtr(robot_state.WO_SELF)
ball = world_objects.getObjPtr(core.WO_BALL)
xVel = 0.0
yVel = 0.0
tVel = 0.0
#Adjust angle
angleToTarget = ball.visionBearing #robot.orientation# * -1.0
if abs(angleToTarget) > 10 * core.DEG_T_RAD and abs(
angleToTarget) < 90.0 * core.DEG_T_RAD:
tVel = 0.5 * max(
min(angleToTarget / 0.87,
0.3), -0.3) #0.25 * (angleToTarget / abs(angleToTarget))
xToTarget = target_x - robot.loc.x
xVel = 0.2
# if abs(xToTarget) > 100:
# xVel = max(0.25 * (xToTarget / abs(xToTarget)),0.1)
yToTarget = target_y - robot.loc.y
if abs(yToTarget) > 100:
yVel = min(max(yToTarget / 200.0, -0.4), 0.4)
tVel = 0.0
else:
xVel = 0.0
tVel = 0.0
print "Goalie Position: {0} {1} {2} Velocity {3} {4} {5}".format(
robot.loc.x, robot.loc.y, core.RAD_T_DEG * robot.orientation, xVel,
yVel, tVel)
commands.setWalkVelocity(xVel, yVel, tVel)
def run(self):
commands.setWalkVelocity(0, 0, -0.25)
if self.getTime() > 6.0:
commands.stand()
if self.getTime() > 7.0:
self.finish()
def run(self):
self.time_current = time.clock()
dt = self.time_current - self.time_last
self.calc_integral(dt)
bearing = self.ball.bearing
distance = self.ball.distance
elevation = core.RAD_T_DEG * self.ball.visionElevation
commands.setHeadPanTilt(bearing, -25.0, 1.5)
if dt == 0:
theta_cont = self.k_t[0] * bearing + self.k_t[
1] * self.theta_integral
dist_cont = self.k_d[0] * (
distance - self.dist) + self.k_d[1] * self.dist_integral
else:
theta_cont = self.k_t[0] * bearing + self.k_t[
1] * self.theta_integral + self.k_t[2] * (bearing -
self.theta_prev) / dt
dist_cont = self.k_d[0] * (distance - self.dist) + self.k_d[
1] * self.dist_integral + self.k_d[2] * (distance -
self.dist_prev) / dt
print("dist cont: %f, dir: %f, theta cont: %f" %
(dist_cont, self.dir, theta_cont))
commands.setWalkVelocity(dist_cont, 0.4 * self.dir, theta_cont)
self.theta_prev = bearing
self.dist_prev = distance
self.time_last = self.time_current
def move_to_position(self, target_x, target_y):
robot = world_objects.getObjPtr(robot_state.WO_SELF);
ball = world_objects.getObjPtr(core.WO_BALL)
xVel = 0.0
yVel = 0.0
tVel = 0.0
#Adjust angle
angleToTarget = ball.visionBearing#robot.orientation# * -1.0
if abs(angleToTarget) > 10 * core.DEG_T_RAD and abs(angleToTarget) < 90.0 * core.DEG_T_RAD:
tVel = 0.5 * max(min(angleToTarget / 0.87,0.3),-0.3) #0.25 * (angleToTarget / abs(angleToTarget))
xToTarget = target_x - robot.loc.x
xVel = 0.2
# if abs(xToTarget) > 100:
# xVel = max(0.25 * (xToTarget / abs(xToTarget)),0.1)
yToTarget = target_y - robot.loc.y
if abs(yToTarget) > 100:
yVel = min(max(yToTarget / 200.0, -0.4),0.4)
tVel = 0.0
else:
xVel = 0.0
tVel = 0.0
print "Goalie Position: {0} {1} {2} Velocity {3} {4} {5}".format(robot.loc.x,robot.loc.y,core.RAD_T_DEG * robot.orientation,xVel,yVel,tVel)
commands.setWalkVelocity(xVel, yVel, tVel)
def run(self):
commands.setWalkVelocity(0, 0, -0.25)
if self.getTime() > 6.0:
commands.stand()
if self.getTime() > 7.0:
self.finish()
def run(self):
# Note x is pixel (left-right) and y is (up-down)
ball = memory.world_objects.getObjPtr(core.WO_BALL)
goal = memory.world_objects.getObjPtr(core.WO_UNKNOWN_GOAL)
dt = 1e-2
# Gain values
# K_P = np.diag([1e-2,1e-3,1e-3])
# K_I = np.diag([1e-3,1e-3,1e-3])
# K_D = np.eye(3)*1e-6
K_Px = 2e-2
K_Py = 2e-3
K_Pt = 1.0
K_Ix = 8e-3
K_Iy = 1e-3
K_It = 1e-2
K_Dx = 1e-6
K_Dy = 1e-6
K_Dt = 1e-5
# Saturation block - low since we are close
max_speed = 0.3
# pos = np.array([ball.imageCenterX,ball.imageCenterY,-goal.visionBearing])
# print("Target shape", self.target.shape," pos shape ", pos.shape)
# err2 = self.target-pos
# print(err2.shape)
err_x2 = self.x_pos - ball.imageCenterX
err_y2 = self.y_pos - ball.imageCenterY
err_t2 = goal.visionBearing - self.theta_pos
dist_error = (float(err_x2)**2 + float(err_y2)**2)**0.5
# dist_error2 = np.linalg.norm(err[0:2])
self.err_x_run += err_x2 * dt
self.err_y_run += err_y2 * dt
self.err_t_run += err_t2 * dt
# print(self.err_run.shape)
# self.err_run += err2*dt
err_x_delta = (err_x2 - self.err_x) / dt
err_y_delta = (err_y2 - self.err_y) / dt
err_t_delta = (err_t2 - self.err_t) / dt
# err_delta = (err2-self.err)/dt
# self.err = err2
self.err_x = err_x2
self.err_y = err_y2
self.err_t = err_t2
# out_com = np.matmul(K_P,self.err.reshape((3,1)))+np.matmul(K_I,self.err_run.reshape(3,1)) + np.matmul(K_D,err_delta.reshape(3,1))
w_dot = K_Px * self.err_x + K_Ix * self.err_x_run + K_Dx * err_x_delta
h_dot = K_Py * self.err_y + K_Iy * self.err_y_run + K_Dy * err_y_delta
t_dot = K_Pt * self.err_t + K_It * self.err_t_run + K_Dt * err_t_delta
command_out = (w_dot**2 + h_dot**2)**0.5
# command_out2 = np.linalg.norm(out_com)
print("Output h: ", h_dot, ", Output w: ", w_dot, "Output t", t_dot)
# print("Output2 h: ",out_com[1],", Output2 w: ",out_com[0],"Output2 t",out_com[2])
commands.setHeadPan(0, 0.1)
commands.setWalkVelocity(min(h_dot, max_speed),
max(min(w_dot, max_speed), -max_speed),
max(min(t_dot, max_speed), -max_speed))
# commands.setWalkVelocity(0,0,max(min(t_dot,max_speed),-max_speed))
print("Distance error : ", dist_error, "Goal angle error: ", err_t2,
" Command out: ", command_out)
def run(self):
"""Keep spinning until you see the ball in either camera"""
memory.speech.say('Spinning!')
ball = memory.world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
self.finish()
commands.setWalkVelocity(0, 0, -0.25)
def run(self):
"""Keep spinning until you see the ball in either camera"""
memory.speech.say('Spinning!')
ball = memory.world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
self.finish()
commands.setWalkVelocity(0, 0, -0.25)
def run(self):
robot = memory.world_objects.getObjPtr(5)
# commands.setHeadTilt(tilt_angle)
global robotlocx, robotlocy
dt = 1e-2
K_Px = 1e-3
K_Py = 1e-3
K_Pt = 1
K_Ix = 1e-4
K_Iy = 1e-4
K_It = 0
K_Dx = 1e-6
K_Dy = 1e-6
K_Dt = 0.0
max_speed = 0.5
# err_x2 = self.goal_x - robot.loc.x#robotlocx
# err_y2 = self.goal_y - robot.loc.y#robotlocy
err_x2 = self.goal_x - robotlocx
err_y2 = self.goal_y - robotlocy
err_list = [robot.orientation - i for i in self.goal_theta]
abs_list = [abs(sk) for sk in err_list]
err_t2 = err_list[abs_list.index(min(abs_list))]
dist_error = (float(err_x2)**2 + float(err_y2)**2)**0.5
self.err_x_run += err_x2 * dt
self.err_y_run += err_y2 * dt
self.err_t_run += err_t2 * dt
err_x_delta = (err_x2 - self.err_x) / dt
err_y_delta = (err_y2 - self.err_y) / dt
err_t_delta = (err_t2 - self.err_t) / dt
self.err_x = err_x2
self.err_y = err_y2
self.err_t = err_t2
# print("x_err: ", self.err_x, " y_err: ",self.err_y)
# if abs(self.err_t) > 3.1415/2:
# print("Angle Fix ",self.err_t)
# f_dot = 0
# h_dot = 0
# a_dot = self.err_t# Angle rate
# else:
# print("Distance fix")
f_dot = (K_Px * self.err_x + K_Ix * self.err_x_run +
K_Dx * err_x_delta) * math.cos(robot.orientation) + (
K_Py * self.err_y + K_Iy * self.err_y_run + K_Dy *
err_y_delta) * math.sin(robot.orientation) # Forward
h_dot = (K_Px * self.err_x + K_Ix * self.err_x_run +
K_Dx * err_x_delta) * math.sin(-robot.orientation) + (
K_Py * self.err_y + K_Iy * self.err_y_run + K_Dy *
err_y_delta) * math.cos(robot.orientation) # Horiztontal
a_dot = 0
# print("h_vel: ", h_dot, " f_vel: ",f_dot)
commands.setWalkVelocity(max(min(f_dot, max_speed), -max_speed),
max(min(h_dot, max_speed), -max_speed),
max(min(a_dot, max_speed), -max_speed))
def run(self):
commands.setHeadPan(0,0.1)
commands.setHeadTilt(tilt_angle)
robot= memory.world_objects.getObjPtr(5)
# memory.speech.say("I'm still turning")
print("Looking for another beacon")
sgn = math.copysign(1,robot.loc.x)*math.copysign(1,robot.orientation)
commands.setWalkVelocity(0,0,sgn*0.1)
def run(self):
commands.setHeadPan(0,0.5)
# commands.setHeadTilt(tilt_angle)
robot= memory.world_objects.getObjPtr(5)
memory.speech.say("Reached center")
print("Reached center")
commands.setWalkVelocity(0,0,0)
print("Final robot ", robot.loc," orientation: ",robot.orientation)
print("Final Position covariance ",robot.sd.getMagnitude(),"cov_or: ",robot.sdOrientation)
def goToDesiredPos(self):
self.time_current = time.clock()
globX = self.robot.loc.x
globY = self.robot.loc.y
globTh = self.robot.orientation
dt = self.time_current - self.time_last
x = self.roboDesiredX
y = self.roboDesiredY
theta = self.roboDesiredTh
bearing = self.ball.bearing
self.calc_integral(dt, x, y, theta)
elevation = core.RAD_T_DEG * self.ball.visionElevation
commands.setHeadPanTilt(self.ball.bearing, -elevation, 1.5)
if dt == 0:
x_cont = self.k_x[0] * (globX - x) + self.k_x[1] * self.x_int
y_cont = self.k_y[0] * (globY - y) + self.k_y[1] * self.y_int
theta_cont = self.k_t[0] * bearing + self.k_t[1] * self.theta_int
else:
x_cont = self.k_x[0] * (globX - x) + self.k_x[
1] * self.x_int + self.k_x[2] * (globX - self.x_prev) / dt
y_cont = self.k_y[0] * (globY - y) + self.k_y[
1] * self.y_int + self.k_y[2] * (globY - self.y_prev) / dt
theta_cont = self.k_t[0] * bearing + self.k_t[
1] * self.theta_int + self.k_t[2] * (bearing -
self.theta_prev) / dt
print("p_cont: %f, i_cont: %f, d_cont: %f" %
(self.k_x[0] *
(globX - x), self.k_x[1] * self.x_int, self.k_x[2] *
(globX - self.x_prev) / dt))
if abs(bearing) >= 0.3:
# Control only the heading of the robot and not the velocity
commands.setWalkVelocity(0.0, 0.0, 0.4 * np.sign(bearing))
else:
# Control both heading and velocity
if self.gb_line_obj.seen:
if globX < 1800.0:
self.gb_line_seg = self.gb_line_obj.lineLoc
gb_line_cent = self.gb_line_seg.getCenter()
rel_robot = core.Point2D(0.0, 0.0)
gb_dist_thresh = 200.0
gb_robo_dist = self.gb_line_seg.getDistanceTo(rel_robot)
# print("Line seen. Center at [%f, %f], robot at [%f, %f] dist to closest point is: %f" % (gb_line_cent.x,gb_line_cent.y,self.robot.loc.x,self.robot.loc.y,gb_robo_dist))
if gb_robo_dist < gb_dist_thresh:
# Too close in either x or y
x_cont = 0.0
# else:
# print("Line not seen.\n")
commands.setWalkVelocity(x_cont, y_cont, theta_cont)
print("Controls: [%f, %f, %f]\n" % (x_cont, y_cont, theta_cont))
self.x_prev = globX - self.x_prev
self.y_prev = globY - self.y_prev
self.theta_prev = bearing
self.time_last = self.time_current
return
def run(self):
global direction, last_direction_change_time
turn_amount = 2.0 * math.pi
turn_vel = 0.2
turn_time = turn_amount / turn_vel
commands.setWalkVelocity(0, 0, turn_vel * direction)
if ((self.getTime() - last_direction_change_time) > turn_time):
direction = direction * -1.0
last_direction_change_time = self.getTime()
def run(self):
global direction, last_direction_change_time
turn_amount = 2.0 * math.pi
turn_vel = 0.2
turn_time = turn_amount / turn_vel
commands.setWalkVelocity(0, 0, turn_vel * direction)
if (self.getTime() - last_direction_change_time) > turn_time:
direction = direction * -1.0
last_direction_change_time = self.getTime()
def run(self):
if not memory.body_model.feet_on_ground_:
memory.speech.say("Flying")
self.reset()
else:
memory.speech.say("Spining")
commands.setHeadPan(0, 1)
# nao = memory.world_objects.getObjPtr(memory.robot_state.WO_SELF)
# t = nao.orientation
# x = nao.loc.x
# y = nao.loc.y
# print("Robot x: {}\t y:{}\t t: {}".format(x, y, t))
# origin = memory.world_objects.getObjPtr(core.WO_TEAM_COACH)
# print("bearing: {}".format(origin.orientation))
beacon_list = [
core.WO_BEACON_BLUE_YELLOW,
core.WO_BEACON_YELLOW_BLUE,
core.WO_BEACON_BLUE_PINK,
core.WO_BEACON_PINK_BLUE,
core.WO_BEACON_PINK_YELLOW,
core.WO_BEACON_YELLOW_PINK,
]
commands.setHeadTilt(-18) # Tilt head up so we can see goal (default = -22)
# commands.setWalkVelocity(0, 0, -0.15)
# if self.getTime() > 5.0:
# any_beacon_seen = False
# for key in beacon_list:
# beacon = memory.world_objects.getObjPtr(key)
# if beacon.seen:
# any_beacon_seen = True
vel_x = 0
vel_y = 0
vel_turn = 0.30
commands.setWalkVelocity(vel_x, vel_y, vel_turn)
if self.getTime() > 10.0:
beacon_x_right_threshold = 360.0 / 2 + 40
beacon_x_left_threshold = 360.0 / 2 - 40
any_beacon_aligned = False
for key in beacon_list:
beacon = memory.world_objects.getObjPtr(key)
if beacon.seen:
if (beacon.imageCenterX < beacon_x_right_threshold) and (
beacon.imageCenterX > beacon_x_left_threshold
):
any_beacon_aligned = True
if any_beacon_aligned:
self.finish()
def run(self):
line = memory.world_objects.getObjPtr(core.WO_OWN_PENALTY)
robot= memory.world_objects.getObjPtr(5)
err_x2 = line.visionDistance - self.target_dist
# Note x is pixel (left-right) and y is (up-down)
ball = memory.world_objects.getObjPtr(core.WO_BALL)
dt = 1e-2
# Gain values
# K_P = np.diag([1e-2,1e-3,1e-3])
# K_I = np.diag([1e-3,1e-3,1e-3])
# K_D = np.eye(3)*1e-6
K_Px = 2e-3
K_Py = 2e-3
K_Pt = 0.0
K_Ix = 2e-3
K_Iy = 1e-3
K_It = 0.0
K_Dx = 0.0
K_Dy = 1e-6
K_Dt = 0.0
# Saturation block - low since we are close
self.err_x_run += err_x2*dt
# print(self.err_run.shape)
# self.err_run += err2*dt
err_x_delta = (err_x2-self.err_x)/dt
# err_delta = (err2-self.err)/dt
# self.err = err2
self.err_x = err_x2
h_dot = K_Px*self.err_x+K_Ix*self.err_x_run+K_Dx*err_x_delta
max_speed = 0.4
self.currt = self.getTime()
err_t2 = ball.bearing-self.theta_pos
self.err_t_run += err_t2*dt
err_t_delta = (err_t2-self.err_t)/dt
self.err_t = err_t2
w_dot = K_Py*self.err_t+K_Iy*self.err_t_run
truew_dot = math.copysign(0.4,err_t2)
truea_dot = math.copysign(0.09,err_t2)
command_out = (truew_dot**2)**0.5
# command_out2 = np.linalg.norm(out_com)
# print(Output w: ",w_dot")
# print("Output2 h: ",out_com[1],", Output2 w: ",out_com[0],"Output2 t",out_com[2])
commands.setHeadPan(0,0.1)
commands.setWalkVelocity(0,truew_dot,truea_dot)
# commands.setWalkVelocity(0,0,max(min(t_dot,max_speed),-max_speed))
# print("Ball angle error: ",err_t2," Command out: ",command_out)
# print("Distance error : ", dist_error," & ",dist_error2)
if abs(err_t2)<=0.04:
# print("Ready to save at: ball at angle ",ball.bearing)
commands.setWalkVelocity(0,0,0)
def defense_walk(self):
global walk_start_time, current_state, DefendingStates
commands.setHeadTilt(-13)
commands.setWalkVelocity(0.3, 0.0, 0.05)
#print "walk time: " + str(walk_start_time)
#print "current time: " + str(self.getTime())
if (self.getTime() - walk_start_time) > 9.0:
current_state = DefendingStates.start
# commands.setWalkVelocity(0.0,0.0,0.0)
commands.stand()
def defense_walk(self):
global walk_start_time, current_state, DefendingStates
commands.setHeadTilt(-13)
commands.setWalkVelocity(0.3,0.0,0.05)
#print "walk time: " + str(walk_start_time)
#print "current time: " + str(self.getTime())
if (self.getTime() - walk_start_time) > 9.0:
current_state = DefendingStates.start
# commands.setWalkVelocity(0.0,0.0,0.0)
commands.stand()
def run(self):
goal = memory.world_objects.getObjPtr(core.WO_OWN_GOAL)
if goal.seen:
commands.setWalkVelocity(0.3, 0,
-0.3 * np.sign(goal.imageCenterX - 500))
print('Goal!')
else:
print('No goal!')
if self.getTime() > 10.0:
self.finish()
def run(self):
commands.setHeadPan(-1, 1)
commands.setWalkVelocity(0.3, 0, 0.3)
nao = memory.world_objects.getObjPtr(memory.robot_state.WO_SELF)
# print nao.loc.x, nao.loc.y, math.tan(nao.orientation)
facing_center = abs(nao.loc.x * math.tan(nao.orientation) - nao.loc.y)
print "Facing center", nao.orientation, facing_center
if self.getTime() > 3.0:
self.finish()
def run(self):
print("circling")
ball = memory.world_objects.getObjPtr(core.WO_BALL)
goal = memory.world_objects.getObjPtr(core.WO_UNKNOWN_GOAL)
commands.setWalkVelocity(0, -0.20, +0.075)
commands.setHeadPan(0, 0.1)
if goal.seen:
print("goal angle ", goal.visionBearing)
if abs(goal.visionBearing) <= 0.02:
commands.setWalkVelocity(0, 0, 0)
self.finish()
def run(self):
# These are max values for Gene to adjust itself
vel_y_gain = -0.50
vel_x_gain = 0.50
vel_turn_gain = 1.00
vel_y = -0.50 # This is the tangential velocity, fix it (try -0.50 cw or left)
vel_x = 0
vel_turn = 0.30
# Change turn direction based on the last seen beacon (ccw or right)
global turn_dir
if turn_dir:
vel_y = 0.50
goal_aligned = False
# Target position of the ball in bottom camera
# # x_desired = 360.0 / 2
# x_desired = 120 # Ball near left foot
# # y_desired = 240.0 / 2
# y_desired = 200 # Ball near the bottom of the frame when looking up
x_desired = 360.0 / 2
y_desired = 220.0
ball = memory.world_objects.getObjPtr(core.WO_BALL)
goal = memory.world_objects.getObjPtr(core.WO_OPP_GOAL)
# Make sure that the ball is always at the same position relative to Gene
if ball.seen:
if ball.fromTopCamera:
memory.speech.say("Align goal The ball is in top frame")
self.postSignal("restart")
else:
# We Calculate the deviation from the center of the frame
# to adjust our rotation speed
# Y determines vel_x to make sure that Gene
# is always a fix distance from the ball
vel_x = vel_x_gain * (y_desired - ball.imageCenterY) / (y_desired)
# X determines the vel_turn. Since we want to always turn
# that's why there's an offset
# vel_turn = 0.25 - (x_desired - ball.imageCenterX) / (x_desired) * 0.25
vel_turn = vel_turn_gain * (x_desired - ball.imageCenterX) / (x_desired)
if goal.seen:
vel_y, goal_aligned = align_goal(vel_y) # Exit condition
if goal_aligned:
self.finish()
# commands.setWalkvelocity(0, -0.50, 0.25)
commands.setWalkVelocity(vel_x, vel_y, vel_turn)
commands.setHeadTilt(-10) # Tilt head up so we can see goal (default = -22)
def run(self):
global last_head_time, last_head_pan , last_keeper_direction
commands.setHeadTilt(-14)
if ((self.getTime() - last_head_time) > 4):
if(last_head_pan >= 0):
last_head_pan = -1
else:
last_head_pan = 1
commands.setHeadPan( last_head_pan, 3.5)
commands.setWalkVelocity(0.2,0.0*last_head_pan,0.05*last_head_pan)
last_head_time = self.getTime()
def run(self):
# Turn head in the other direction
if self.getTime >= 3.0:
self.resetTime()
if WalkToCenter.direction == WalkToCenter.LEFT:
WalkToCenter.direction = WalkToCenter.RIGHT
else:
WalkToCenter.direction = WalkToCenter.LEFT
# Look left or right
commands.setHeadPan(WalkToCenter.direction, 1)
# Calculate the angle of the vector to origin in robot's frame
nao = memory.world_objects.getObjPtr(memory.robot_state.WO_SELF)
t, x, y = nao.orientation, nao.loc.x, nao.loc.y
speed = 0.5
dx, dy = 0 - x, 0 - y
angle = math.atan(dy / dx)
# Quadrant 1
if x > 0 and y > 0:
angle += math.pi
# Quadrant 2
if x < 0 and y > 0:
angle += 0
# Quadrant 3
if x < 0 and y < 0:
angle += 0
# Quadrant 4
if x > 0 and y < 0:
angle += math.pi
# Take the robot's orientation into account
angle -= t
# Calculate forward velocity
distance_to_center = math.sqrt(x ** 2 + y ** 2) / 1000
DISTANCE_THRESHOLD = 1.5
distance_to_center = min(distance_to_center, DISTANCE_THRESHOLD)
DISTANCE_CONSTANT = 2 / 3.0
forward_vel = distance_to_center * DISTANCE_CONSTANT
MAX_FORWARD_VELOCITY = 0.50
forward_vel *= MAX_FORWARD_VELOCITY
MIN_FORWARD_VELOCITY = 0.35
forward_vel = max(MIN_FORWARD_VELOCITY, forward_vel)
# Walk towards the center!
print forward_vel, angle
MAX_ANGLE_VELOCITY = 0.3
turning_vel = max(-MAX_ANGLE_VELOCITY, min(MAX_ANGLE_VELOCITY, angle))
commands.setWalkVelocity(forward_vel, 0, turning_vel)
def run(self):
commands.setHeadTilt(-10) # Tilt head up so we can see goal (default = -22)
memory.speech.say("Turning!")
commands.setWalkVelocity(0, 0, -0.15)
if self.getTime() > 5.0:
any_beacon_seen = False
for key in BEACON_LIST:
beacon = memory.world_objects.getObjPtr(key)
if beacon.seen:
any_beacon_seen = True
if any_beacon_seen:
self.finish()
def run(self):
ball = world_objects.getObjPtr(core.WO_BALL)
goal = world_objects.getObjPtr(core.WO_UNKNOWN_GOAL)
if ball.seen and goal.seen and abs(goal.visionBearing) < 10 * core.DEG_T_RAD:
self.finishCt += 1
else:
self.finishCt = max(0,self.finishCt - 1)
tVel = max(min(1.0,ball.visionBearing / 0.87),-1.0)
xVel = max(min(1.0, 0.4 * ball.visionDistance / 300), -1.0)
print "Ball: {0} {1}".format(ball.visionDistance,ball.visionBearing * core.RAD_T_DEG)
print "Rotate Velocity: {0} {1}".format(xVel,tVel)
commands.setWalkVelocity(xVel,-0.3,tVel)
if self.finishCt > 5:
self.finish()
def toCenter(self, dir):
vel_x = 0.4
vel_y = 0
vel_turn = 0.05 # This is needed because forward odo is not reliable
turn_speed = 0.3
origin = memory.world_objects.getObjPtr(core.WO_TEAM_COACH)
bearing = origin.orientation
distance = origin.distance
# Calculate forward velocity
dist = distance / 1000.0
DIST_THRESHOLD = 1.5
dist = min(dist, DIST_THRESHOLD)
DIST_CONSTANT = 2 / 3.0
vel_x = dist * DIST_CONSTANT
MAX_VEL_XOCITY = 0.50
vel_x *= MAX_VEL_XOCITY
MIN_VEL_XOCITY = 0.30
vel_x = max(MIN_VEL_XOCITY, vel_x)
# Walk towards the center!
MAX_BEARING_VELOCITY = 0.3
vel_turn = max(-MAX_BEARING_VELOCITY, min(MAX_BEARING_VELOCITY, bearing))
# if (bearing > 0):
# vel_turn += turn_speed
# else:
# vel_turn -= turn_speed
commands.setHeadTilt(-18) # Tilt head up so we can see goal (default = -22)
commands.setWalkVelocity(vel_x, vel_y, vel_turn)
# commands.setHeadPan(dir * 1.5, 1)
print ("distance: {}\t\t bearing: {}".format(distance, bearing))
# Kidnapped relocalize
if not memory.body_model.feet_on_ground_:
ToCenterBoth.num_times_no_beacon = 0
self.postSignal("spin")
print ("flying")
# Closer
if distance < 50:
ToCenterBoth.num_times_no_beacon = 0
self.postSignal("check")
def run(self):
nao = memory.world_objects.getObjPtr(memory.robot_state.WO_SELF)
# Calculate the angle of the vector to origin in robot's frame
t = nao.orientation
x = nao.loc.x
y = nao.loc.y
speed = 0.4
dx = 0 - x
dy = 0 - y
angle = math.atan(dy / dx)
# Quadrant 1
if x > 0 and y > 0:
angle += math.pi
# Quadrant 2
if x < 0 and y > 0:
angle += 0
# Quadrant 3
if x < 0 and y < 0:
angle += 0
# Quadrant 4
if x > 0 and y < 0:
angle += math.pi
# Take the robot's orientation into account
angle -= t
vel_turn = 0.3 # Constant spin velocity
# vel_turn = 0.0 # Constant spin velocity
vel_y = -(speed * math.sin(angle))
vel_x = +(speed * math.cos(angle))
print ("Robot x: {}\t y:{}\t t: {}".format(x, y, t))
# print("vel_x: {}\t vel_y: {}\t angle: {}".format(vel_x, vel_y, angle))
vel_x = 0
vel_y = 0
commands.setHeadTilt(0) # Tilt head up so we can see goal (default = -22)
commands.setWalkVelocity(vel_x, vel_y, vel_turn)
if self.getTime() > 30.0:
self.finish()
def run(self):
ball = world_objects.getObjPtr(core.WO_BALL)
if ball.seen and ball.visionDistance < 1000:
print "Ball Distance: ", ball.visionDistance
self.last_seen = 0
ball_x, ball_y = self.get_object_position(ball)
if ball_x < 100 and abs(ball_y - self.kick_offset) < 30:
self.finishCt += 1
else:
self.finishCt = max(self.finishCt - 1, 0)
xVel = 0.0
if ball_x >= 100:
xVel = self.x.update(ball_x)
yVel = 0.0
if abs(ball_y - self.kick_offset) >= 30:
yVel = self.y.update(ball_y - self.kick_offset)
tVel = (ball.visionBearing - self.initialTheta) / 0.87
xVel = max(min(xVel,0.5),-0.5)
yVel = max(min(yVel,1.0),-1.0)
tVel = 0#max(min(yVel,0.1),-0.1)
if tVel * yVel < 0:
tVel = tVel * -1
print "Final: {0}, {1}, {2} --> Walk Velocity: {3}, {4}, {5}".format(ball_x, ball_y - self.kick_offset, ball.visionBearing - self.initialTheta, xVel, yVel, tVel)
commands.setWalkVelocity(xVel, yVel, tVel)
# memory.walk_request.setLineUp(0.0,True)
else:
self.last_seen += 1
if self.last_seen > 10:
commands.setWalkVelocity(0, 0, 0)
self.finishCt = max(self.finishCt - 1, 0)
if self.finishCt > 3:
selfRobot = world_objects.getObjPtr(robot_state.WO_SELF);
self.x.reset()
self.y.reset()
if selfRobot.loc.x < 5000:
self.postSignal("kick")
else:
self.postSignal("dribble")
self.finish()
def run(self):
goal = world_objects.getObjPtr(core.WO_UNKNOWN_GOAL)
ball = world_objects.getObjPtr(core.WO_BALL)
if ball.seen and goal.seen:
print "Ball Distance: ", ball.visionDistance
self.last_seen = 0
self.target_pos = self.get_target_position(goal, ball)
print "Target Position: {0}, {1}, {2}".format(self.target_pos[0], self.target_pos[1], math.degrees(self.target_pos[2])) #, self.target_pos
(x, y, t) = self.target_pos
if (x < 50 and abs(y) < 40 and abs(math.degrees(t)) < 10): # 50 40 10
self.finish()
update = lambda e, pid: pid.update(e)
control = map(lambda x: update(*x), zip(self.target_pos, self.controller))
print "Setting Walk Velocity: ", control
commands.setWalkVelocity(*control)
else:
self.last_seen += 1
if self.last_seen > 10:
print "Search!"
commands.setWalkVelocity(0, 0, 0.3)
def run(self):
"""Approach the ball and set up for a kick
If the ball is seen in the top camera, the error term in the distance of
the ball.
"""
ball = memory.world_objects.getObjPtr(core.WO_BALL)
if not ball.seen or not ball.fromTopCamera:
return
# Ball coordinates
ball_x, ball_y = ball.imageCenterX, ball.imageCenterY
# Calculate forward velocity
ball_distance = ball.visionDistance / 1000
print("Ball distance: {}".format(ball_distance))
ball_distance = min(ball_distance, DISTANCE_THRESHOLD)
# Cache the ball distances
PursueBall.ball_distances = (PursueBall.ball_distances + [ball_distance])[-30:]
print("Ball distances: {}".format(PursueBall.ball_distances))
slope = sum(PursueBall.ball_distances[-10:]) / 10 - sum(PursueBall.ball_distances[:10]) / 10
print(
"Slope: {} - {} = {}".format(
sum(PursueBall.ball_distances[-10:]) / 10, sum(PursueBall.ball_distances[:10]) / 10, slope
)
)
print("Input: {}".format(1 / slope if slope else 1))
# Get the maximum velocity to be 1
forward_vel = ball_distance * DISTANCE_CONSTANT
forward_vel *= MAX_FORWARD_VELOCITY
forward_vel = max(MIN_FORWARD_VELOCITY, forward_vel)
print("forward velocity: {}".format(forward_vel))
# Calculate sideways velocity
angular_vel = -(ball_x - 160.0) / 160.0 * MAX_ANGULAR_VELOCITY
print("Sideways Amount: {}".format(angular_vel))
commands.setWalkVelocity(forward_vel, 0, angular_vel)
def run(self):
seen = False
for b in self.beacons:
beacon = world_objects.getObjPtr(b)
if beacon.seen:
seen = True
break
if seen:
self.seenCt += 1
else:
self.seenCt = max(self.seenCt - 1, 0)
if self.seenCt > 5:
self.seenCt = 0
self.finish()
commands.setWalkVelocity(0.0,0.0,0.5)
# commands.setHeadTilt(-18)
commands.setHeadPan(0.0,2.0)
def run(self):
ball = memory.world_objects.getObjPtr(core.WO_BALL)
print "aaaaaaaaa"
if ball.seen:
#print "ball.visionBearing is %f!" % ball.visionBearing
print "I have seen the blue goal"
commands.setStiffness()
if ball.visionBearing > 0.20:
commands.setWalkVelocity(0, 0, 0.3)
elif ball.visionBearing < -0.20:
commands.setWalkVelocity(0, 0, -0.3)
else:
print "ball.visionDistance is %f!" % ball.visionDistance
if ball.visionDistance > 2200:
commands.setWalkVelocity(0.6, 0, 0)
elif ball.visionDistance < 1500:
commands.setWalkVelocity(0, 0, 0)
else:
commands.setWalkVelocity(0, 0, 0)
else:
print "I don't see anything"
def turn(self):
# memory.speech.say("Turning!")
global have_lock, facing_center, at_center
sloc = mem_objects.world_objects[robot_state.WO_SELF].loc
t = -mem_objects.world_objects[robot_state.WO_SELF].orientation
cx = (-sloc.x) * numpy.cos(t) - (-sloc.y) * numpy.sin(t)
cy = (-sloc.x) * numpy.sin(t) + (-sloc.y) * numpy.cos(t)
# print "global x,y,t = " + str(sloc.x) + "," + str(sloc.y) + "," + str(t)
# print "local center x,y = " + str(cx) + "," + str(cy)
t_err = numpy.arctan2(cy, cx)
print "t_err = " + str(t_err)
if numpy.abs(t_err) < 0.05:
facing_center = True
return
else:
Kt = 1.0
vt_max = 0.25
t_vel = vt_max * numpy.tanh(Kt * t_err);
print "t_vel = " + str(t_vel)
commands.setWalkVelocity(0.0, 0.0, t_vel)
def search(self):
global direction, last_direction_change_time, have_lock, facing_center, at_center, beacons_seen
# memory.speech.say("Searching!")
for i in xrange(core.WO_BEACON_BLUE_YELLOW, core.WO_BEACON_YELLOW_PINK):
if(mem_objects.world_objects[i].seen):
beacons_seen.add(i)
print "saw beacon " + str(i)
if(len(beacons_seen) >= num_beacons_required):
# memory.speech.say("Done")
have_lock = True
return
else:
print "Waiting for " + str(num_beacons_required - len(beacons_seen)) + " more beacons"
turn_amount = 2.0*math.pi
turn_vel = 0.2
turn_time = turn_amount / turn_vel
commands.setWalkVelocity(0, 0, turn_vel * direction)
if((self.getTime() - last_direction_change_time) > turn_time):
direction = direction * -1.0
last_direction_change_time = self.getTime()
def walk(self):
global have_lock, facing_center, at_center
# memory.speech.say("Walking!")
sloc = mem_objects.world_objects[robot_state.WO_SELF].loc
t = -mem_objects.world_objects[robot_state.WO_SELF].orientation
cx = (-sloc.x) * numpy.cos(t) - (-sloc.y) * numpy.sin(t)
cy = (-sloc.x) * numpy.sin(t) + (-sloc.y) * numpy.cos(t)
t_err = numpy.arctan2(cy, cx)
# print "global x,y,t = " + str(sloc.x) + "," + str(sloc.y) + "," + str(t)
print "local center x,y = " + str(cx) + "," + str(cy)
Kx = 10.0
vx_max = 0.4
x_vel = vx_max * numpy.tanh(Kx * cx / 1000.0)
Ky = 10.0
vy_max = 0.4
y_vel = vy_max * numpy.tanh(Ky * cy / 1000.0)
t_vel = 0.0
if ((x_vel * x_vel + y_vel * y_vel) < 0.1):
t_vel = 0.1
print "vel x,y,t = " + str(x_vel) + "," + str(y_vel) + "," + str(t_vel)
commands.setWalkVelocity(x_vel, y_vel, t_vel)