def run(self):
commands.standStraight()
#commands.setHeadTilt(0)
commands.setHeadPan(1.5, 3)
if self.getTime() > 5.0:
memory.speech.say("Init head complete!")
self.finish()
def run(self):
commands.stand()
commands.setHeadPan(self.direction * 50 * core.DEG_T_RAD, 0.5)
if abs(core.joint_values[core.HeadYaw]) > 45 * core.DEG_T_RAD:
if core.joint_values[core.HeadYaw] > 0:
self.direction = -1
else:
self.direction = 1
ct = 0
seen = False
for b in self.beacons:
beacon = world_objects.getObjPtr(b)
if beacon.seen:
seen = True
ct += 1
if ct >= 2:
self.finish()
if seen:
self.seenCt += 1
else:
self.seenCt = max(self.seenCt - 1, 0)
if self.seenCt > 5:
self.seenCt = 0
self.finish()
def run(self):
commands.stand()
commands.setHeadPan(self.direction * 50 * core.DEG_T_RAD, 0.5)
if abs(core.joint_values[core.HeadYaw]) > 45 * core.DEG_T_RAD:
if core.joint_values[core.HeadYaw] > 0:
self.direction = -1
else:
self.direction = 1
ct = 0
seen = False
for b in self.beacons:
beacon = world_objects.getObjPtr(b)
if beacon.seen:
seen = True
ct += 1
if ct >= 2:
self.finish()
if seen:
self.seenCt += 1
else:
self.seenCt = max(self.seenCt - 1, 0)
if self.seenCt > 5:
self.seenCt = 0
self.finish()
def run(self):
pose.Sit()
commands.setHeadPan(bearing, 0.1)
ball = mem_objects.world_objects[core.WO_BALL]
bearing = ball.bearing
x = ball.loc.x
y = ball.loc.y
d = ball.distance
xv = ball.absVel.x
yv = ball.absVel.y
if x < 500:
if xv > -10:
print "ball moving too slowly"
return
print "Ball is close, blocking!\n"
if ball.bearing > 30 * core.DEG_T_RAD:
choice = "left"
print "left\n"
elif ball.bearing < -30 * core.DEG_T_RAD:
choice = "right"
print "right\n"
else:
choice = "center"
print "center\n"
self.postSignal(choice)
def run(self):
commands.setHeadPan(0.0,0.2)
ball = world_objects.getObjPtr(core.WO_BALL)
if ball.seen and ball.visionDistance < 3000:
print "Ball Distance: ", ball.visionDistance
self.last_seen = 0
ball_x, ball_y = self.get_object_position(ball)
bearing = ball.visionBearing;
if ball_x < 500 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):
commands.standStraight()
#commands.setHeadTilt(0)
commands.setHeadPan(1.5, 3)
if self.getTime() > 5.0:
memory.speech.say("Init head complete!")
self.finish()
def run(self):
global carrot, direction, seen_ball, last_ball_heading, last_ball_time, alpha, last_carrot
ball = memory.world_objects.getObjPtr(core.WO_BALL)
commands.setStiffness()
feedback_rate = 0.2
velocity = 2.0
if (ball.seen):
memory.speech.say("found ball")
carrot = ball.visionBearing
if (seen_ball):
dt = self.getTime() - last_ball_time
ball_vel = (ball.visionBearing - last_ball_heading) / dt
direction = numpy.sign(ball_vel)
else:
last_carrot = carrot
seen_ball = True
last_ball_time = self.getTime()
last_ball_heading = ball.visionBearing
else:
# memory.speech.say("searching")
carrot += 0.01 * direction
if (carrot > (math.pi / 2.0 - 0.1) and direction > 0.0):
direction = -1.0
elif (carrot < -(math.pi / 2.0 - 0.1) and direction < 0.0):
direction = 1.0
last_carrot = alpha * last_carrot + (1 - alpha) * carrot
delta = abs(last_carrot - core.joint_values[core.HeadYaw])
dt = numpy.maximum(0.1, delta / velocity)
commands.setHeadPan(last_carrot, dt)
def run(self):
commands.setHeadPan(self.direction * 45 * core.DEG_T_RAD, self.time)
if self.tilt is not None:
commands.setHeadTilt(self.tilt)
if self.getTime() > self.time:
self.direction *= -1
def run(self):
global carrot, direction, seen_ball, last_ball_heading, last_ball_time, alpha, last_carrot
ball = memory.world_objects.getObjPtr(core.WO_BALL)
commands.setStiffness()
feedback_rate = 0.2
velocity = 2.0
if(ball.seen):
memory.speech.say("found ball")
carrot = ball.visionBearing
if(seen_ball):
dt = self.getTime() - last_ball_time
ball_vel = (ball.visionBearing - last_ball_heading) / dt
direction = numpy.sign(ball_vel)
else:
last_carrot = carrot
seen_ball = True
last_ball_time = self.getTime()
last_ball_heading = ball.visionBearing
else:
# memory.speech.say("searching")
carrot += 0.01*direction
if(carrot > (math.pi/2.0-0.1) and direction > 0.0):
direction = -1.0
elif(carrot < -(math.pi/2.0-0.1) and direction < 0.0):
direction = 1.0
last_carrot = alpha * last_carrot + (1-alpha) * carrot
delta = abs(last_carrot - core.joint_values[core.HeadYaw])
dt = numpy.maximum(0.1, delta / velocity)
commands.setHeadPan(last_carrot, dt)
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):
commands.setStiffness()
commands.setsetWalkVelocity(.5, 0, 0.0)
commands.setHeadPan(math.pi/4.0, 3)
commands.setHeadPan(-math.pi/4.0, 3)
if self.getTime() > 15.0:
self.finish()
def run(self):
commands.setHeadPan(0.0, 0.2)
ball = world_objects.getObjPtr(core.WO_BALL)
if ball.seen and ball.visionDistance < 3000:
print "Ball Distance: ", ball.visionDistance
self.last_seen = 0
ball_x, ball_y = self.get_object_position(ball)
bearing = ball.visionBearing
if ball_x < 500 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):
commands.setHeadPan(self.pan, self.time)
if self.tilt is not None:
commands.setHeadTilt(self.tilt)
if self.getTime() > self.time:
self.finish()
def run(self):
commands.setHeadPan(self.direction * 45 * core.DEG_T_RAD, self.time)
if self.tilt is not None:
commands.setHeadTilt(self.tilt)
if self.getTime() > self.time:
self.direction *= -1
def run(self):
self.time_current = time.clock()
dt = self.time_current - self.time_last
if (dt > 1.0):
self.dir = self.dir * (-1.0)
dt = 0.0
self.checkLocalized()
if self.localized:
if not self.initialized:
self.goalieStartX = self.robot.loc.x
self.goalieStartY = self.robot.loc.y
self.goalieStartTh = self.robot.orientation
self.initialized = True
self.getDesiredGoaliePos()
self.goToDesiredPos()
# Look at the ball
if self.ball.seen:
# if self.getTime():
commands.setHeadPan(
self.ball.bearing + self.dir * 25.0 * core.DEG_T_RAD, 0.2)
# print("Robot pose: [%f,%f,%f]\n" %(robot.loc.x,robot.loc.y,robot.orientation*core.RAD_T_DEG))
if self.getTime() > 2.0:
if self.localized:
signal = "localized"
else:
signal = "lost"
self.postSignal(signal)
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):
# 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):
if self.getTime() < 1.0:
memory.speech.say("Blocker!")
ball = mem_objects.world_objects[core.WO_BALL]
commands.setHeadPan(ball.bearing, 1.0)
UTdebug.log(15, ".")
# Store a list of previous velocities
if ball.seen:
UTdebug.log(15, "seen")
ball_x, ball_y = ball.imageCenterX, ball.imageCenterY
x_head_turn = -(ball_x-(320.0 / 2.0)) / 160.0
commands.setHeadPan(x_head_turn, .5)
# Max size is 6
if len(self.vel_list) > 3:
self.vel_list = self.vel_list[:-1]
temp_list = [ ball.absVel.x ]
self.vel_list = temp_list + self.vel_list
y_intersect = (ball.absVel.y / ball.absVel.x) * (-600 - ball.loc.x) + ball.loc.y
UTdebug.log(15, "y_intersect: {}".format(y_intersect))
if ball.distance < 750:
# TODO
accel = (self.vel_list[-1] - self.vel_list[0]) / float(len(self.vel_list))
# ball_loc_pred = abs(ball.absVel.x * 3)
t = 3
ball_loc_pred = (ball.absVel.x * t) + ((t * t) / 2 * accel)
# if abs(ball.absVel.x * 3) > ball.distance:
if True:
# Find left or right or center block
y_intersect = (ball.absVel.y / ball.absVel.x) * (-750 - ball.loc.x) + ball.loc.y
UTdebug.log(15, "distance: {}\t ball_loc_pred: {}\t ball.absVel.x: {}".format(ball.distance, ball_loc_pred, ball.absVel.x))
UTdebug.log(15, "accel: {}".format(accel))
UTdebug.log(15, "Ball is close, blocking!")
print("y_intersect: {}".format(y_intersect))
xvel_avg = sum(self.vel_list) / len(self.vel_list)
print('XVEL_LENGTH = {}'.format(len(self.vel_list)))
print('XVEL_AVG = {}'.format(xvel_avg))
min_xvel_avg = -650
# if ball.bearing > 30 * core.DEG_T_RAD:
if xvel_avg < min_xvel_avg:
if 0 < y_intersect < 500:
choice = "left"
# elif ball.bearing < -30 * core.DEG_T_RAD:
elif -500 < y_intersect < -0:
choice = "right"
else:
choice = "left"
self.vel_list = []
self.postSignal(choice)
else:
self.vel_list = []
def run(self):
commands.setHeadPan(1.5, 10)
goal = memory.world_objects.getObjPtr(core.WO_UNKNOWN_GOAL)
solo_post = memory.world_objects.getObjPtr(core.WO_UNKNOWN_GOALPOST)
if goal.seen:
goal_x, goal_y = goal.imageCenterX, goal.imageCenterY
solo_post.imageCenterX = 163
print(solo_post.imageCenterX)
def run(self):
commands.setHeadPan(1.5, 10)
goal = memory.world_objects.getObjPtr(core.WO_UNKNOWN_GOAL)
solo_post = memory.world_objects.getObjPtr(core.WO_UNKNOWN_GOALPOST)
if goal.seen:
goal_x, goal_y = goal.imageCenterX, goal.imageCenterY
solo_post.imageCenterX = 163
print(solo_post.imageCenterX)
def run(self):
ball = mem_objects.world_objects[core.WO_BALL]
commands.setStiffness()
if ball.seen:
# commands.setHeadPanTilt(ball.visionBearing, ball.visionElevation, 0.5)
commands.setHeadPan(ball.visionBearing, time_delay)
print(ball.visionBearing, ball.visionElevation)
print(ball.seen, ball.imageCenterX, ball.imageCenterY)
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):
#ball = mem_objects.world_objects[core.WO_BALL]
#if ball.seen:
# self.finish()
#else:
commands.setStiffness()
commands.setHeadPan(math.pi / 3, time_delay)
if self.getTime() > time_delay:
self.finish()
def run(self):
commands.stand()
commands.setHeadPan(self.direction * 50 * core.DEG_T_RAD, 0.5)
if abs(core.joint_values[core.HeadYaw]) > 45 * core.DEG_T_RAD:
if core.joint_values[core.HeadYaw] > 0:
self.direction = -1
else:
self.direction = 1
def run(self):
commands.stand()
commands.setHeadPan(self.direction * 50 * core.DEG_T_RAD, 0.5)
if abs(core.joint_values[core.HeadYaw]) > 45 * core.DEG_T_RAD:
if core.joint_values[core.HeadYaw] > 0:
self.direction = -1
else:
self.direction = 1
def run(self):
ball = mem_objects.world_objects[core.WO_BALL]
#commands.setStiffness()
if ball.seen:
commands.setHeadPan(ball.visionBearing, ball_turn_delay)
print("=======================================")
print(ball.visionBearing, ball.visionElevation)
print(ball.seen, ball.imageCenterX, ball.imageCenterY)
print("=======================================")
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 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 run(self):
commands.setHeadPan(-1, 1)
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):
commands.setHeadPan(-1.0, 3)
ball = memory.world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
commands.setHeadPan(core.joint_values[core.HeadPan], 3)
self.postSignal("right")
if self.getTime() > 3.0:
self.resetTime()
self.finish()
def run(self):
commands.setHeadPan(-1.0, 3)
ball = memory.world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
commands.setHeadPan(core.joint_values[core.HeadPan], 3)
self.postSignal("right")
if self.getTime() > 3.0:
self.resetTime()
self.finish()
def run(self):
commands.setHeadPan(-1, 1)
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):
ball = memory.world_objects.getObjPtr(core.WO_BALL)
print "I am tracking"
if ball.seen:
print "I see the ball"
commands.setStiffness()
#commands.setHeadPan(ball.visionBearing, 0.1)
if ball.visionBearing > angle:
commands.setHeadPan(ball.visionBearing, (ball.visionBearing-angle)/speed)
else:
commands.setHeadPan(ball.visionBearing, -(ball.visionBearing-angle)/speed)
def run(self):
if int(self.getTime()) % 4 >= 2:
memory.speech.say("left")
commands.setHeadPan(1.5, 1.5)
else:
memory.speech.say("right")
commands.setHeadPan(-1.5, 1.5)
ball = memory.world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
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):
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):
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):
ball = memory.world_objects.getObjPtr(core.WO_BALL)
goal = memory.world_objects.getObjPtr(core.WO_UNKNOWN_GOAL)
commands.setHeadPan(0, 0.1)
#print("Goal hold angle ",self.goalhold.visionBearing)
#print("Head tilt ",core.joint_values[core.HeadPitch])
if ball.seen:
print("Ball at ", ball.imageCenterX, ", ", ball.imageCenterY,
"Bearing ", ball.visionBearing)
if goal.seen:
print("Goal at ", goal.imageCenterX, ", ", goal.imageCenterY,
"Bearing ", goal.visionBearing)
def run(self):
ball = mem_objects.world_objects[core.WO_BALL]
commands.setHeadPan(ball.bearing, 0.1)
if ball.distance < 500:
UTdebug.log(15, "Ball is close, blocking!")
if ball.bearing > 30 * core.DEG_T_RAD:
choice = "left"
elif ball.bearing < -30 * core.DEG_T_RAD:
choice = "right"
else:
choice = "center"
self.postSignal(choice)
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.
forward_vel = distance_to_center * DISTANCE_CONSTANT
MAX_FORWARD_VELOCITY = .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 = .3
turning_vel = max(-MAX_ANGLE_VELOCITY, min(MAX_ANGLE_VELOCITY, angle))
commands.setWalkVelocity(forward_vel, 0, turning_vel)
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.setWalkVelocity(0.2, 0, 0)
#commands.setHeadPan(1, 1.0)
ball = memory.world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
#memory.speech.say('Yes Yes Yes Yes')
#commands.setHeadPan(1, 1.0)
commands.setStiffness()
print "ball.visionBearing is %f!" % ball.visionBearing
commands.setHeadPan(ball.visionBearing, 0.1)
print "Yes Yes Yes Yes"
else:
#memory.speech.say('NO')
print "NO"
def run(self):
dt = self.getTime() - self.prevTime
# if not ball.seen, move the head back to the center
ball = mem_objects.world_objects[core.WO_BALL]
if not ball.seen:
commands.setHeadPan(0.0, 0.2)
else:
commands.setHeadPan(ball.bearing, 0.2)
# print("Ball distance: %f Ball bearing: %f\n" % (ball.distance,ball.bearing))
v_mag = math.sqrt(ball.relVel.x * ball.relVel.x +
ball.relVel.y * ball.relVel.y)
x_ball = ball.distance * math.cos(ball.bearing) / 10.0
y_ball = ball.distance * math.sin(ball.bearing) / 10.0
delta_t = 2.0
v_thresh = -(x_ball / delta_t)
print("V Thresh: %f" % (v_thresh))
disp = odometry.displacement
if not dt == 0:
print("Disp: [%f, %f, %f]" %
(0.1 * disp.translation.x / dt,
0.1 * disp.translation.y / dt, 0.1 * disp.rotation / dt))
# print("X: %f, Y: %f, VThresh: %f, VMag: %f, Vx: %f, Vy: %f" % (x_ball, y_ball, v_thresh, v_mag, ball.relVel.x, ball.relVel.y))
self.prevTime = self.getTime()
if ball.relVel.x < v_thresh and ball.seen and ball.relVel.x < 0.0:
# ball moving away from the robot, reset to regular position
print("Current Vx: %f, Current Vy: %f, x ball: %f, y ball: %f" %
(ball.relVel.x, ball.relVel.y, x_ball, y_ball))
y_intercept = -ball.relVel.y * x_ball / ball.relVel.x + y_ball
print("Y intercept: %f" % y_intercept)
## can do if xvel < yvel && xvel < 1, etc.
## maybe look at distance between robot and goal?
UTdebug.log(15, "Ball is close, blocking!")
if y_intercept > 45.0 or y_intercept < -45.0 or not ball.seen:
choice = "miss"
elif y_intercept > 18.5:
choice = "left"
elif y_intercept < -18.5:
choice = "right"
elif y_intercept <= 18.5 and y_intercept >= -18.5:
choice = "center"
self.postSignal(choice)
return
elif ball.seen:
choice = "moveBall"
# print("Move ball signal")
self.postSignal(choice)
return
else:
return
def run(self):
ball = mem_objects.world_objects[core.WO_BALL]
if ball.seen:
self.finish()
else:
pan = core.joint_values[core.HeadPan]
if abs(pan - math.pi /
3) < eps: #if within eps from full right, turn left
commands.setHeadPan(-math.pi / 3, time_delay)
elif abs(pan + math.pi /
3) < eps: #if within eps from full left, turn right
commands.setHeadPan(math.pi / 3, time_delay)
print("=======================================")
print(pan)
print("=======================================")
def run(self):
print "Track {0}".format(self.lostCount)
commands.stand()
ball = world_objects.getObjPtr(core.WO_BALL)
angle = core.joint_values[core.HeadYaw]
if ball.seen:
self.lostCount = max(0,self.lostCount - 1)
angle = ball.visionBearing
print angle
commands.setHeadPan(angle, 0.2)
else:
self.lostCount = self.lostCount + 1
if self.lostCount > 7:
self.lostCount = 0
self.finish()
def run(self):
if any(tuple(visible_beacons())):
ToCenterBoth.num_times_no_beacon = 0
else:
ToCenterBoth.num_times_no_beacon += 1
if ToCenterBoth.num_times_no_beacon > 30 * 5:
self.postSignal("spin")
if (int(self.getTime()) % 4) >= 2:
memory.speech.say("left")
commands.setHeadPan(1.5, 1.5)
else:
memory.speech.say("right")
commands.setHeadPan(-1.5, 1.5)
toCenter(self, 0)
def run(self):
commands.setHeadPan(1.0, 1)
for key, value in self.beacon_list.iteritems():
# Skip ones we have already seen
if value:
continue
beacon = memory.world_objects.getObjPtr(key)
if beacon.seen:
commands.setHeadPan(core.joint_values[core.HeadPan], 3)
self.beacon_list[key] = 2
# self.beacon_list["last"] = key
print("BEACOOOOON")
self.postSignal("left")
break
if self.getTime() > 3.0:
self.finish()
def run(self):
ball = memory.world_objects.getObjPtr(core.WO_BALL)
if not ball.seen:
return
ball_x, ball_y = ball.imageCenterX, ball.imageCenterY
x_head_turn = -(ball_x-(320.0 / 2.0)) / 160.0
commands.setHeadPan(x_head_turn, 1)
if self.getTime() < 5.0:
return
BEARING_THRESHOLD = .3
if abs(ball.bearing) > BEARING_THRESHOLD:
if ball.bearing < 0:
self.postSignal("right")
elif ball.bearing > 0:
self.postSignal("left")
def run(self):
print "Search: {0}, {1}".format(self.direction, self.seenCount)
commands.setStiffness(cfgstiff.One)
commands.stand()
current_angle = core.joint_values[core.HeadYaw]
commands.setHeadPan(self.direction * 55 * DEG_T_RAD, 1.0)
ball = world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
self.seenCount = self.seenCount + 1
else:
self.seenCount = max(0,self.seenCount - 1)
if self.direction == 1 and current_angle > 45 * DEG_T_RAD:
self.direction = -1
if self.direction == -1 and current_angle < -45 * DEG_T_RAD:
self.direction = 1
if self.seenCount > 3:
self.seenCount = 0
self.finish()
print self.seenCount
def run(self):
# Stand straight up because the head can't move if crouching
ball = memory.world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
# memory.speech.say("I see the ball!")
ball_x, ball_y = ball.imageCenterX, ball.imageCenterY
# print('Ball seen at ({},{})'.format(ball_x, ball_y))
x_head_turn = -(ball_x-(320.0 / 2.0)) / 160.0
# print('X HEAD TURN: {}'.format(x_head_turn))
commands.setHeadPan(x_head_turn, 1)
# y_head_tilt = -(ball_y-(240.0 / 2.0)) / 120.0 * 30
# # print('Y HEAD TILT: {}'.format(y_head_tilt))
# commands.setHeadTilt(y_head_tilt)
self.reset()
if self.getTime() > 3.0:
self.postSignal(self.inSignal())
def run(self):
# Stand straight up because the head can't move if crouching
commands.standStraight()
ball = memory.world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
memory.speech.say("I see the ball!")
print('Ball seen!!!!')
ball_x, ball_y = ball.imageCenterX, ball.imageCenterY
print('Ball seen at ({},{})'.format(ball_x, ball_y))
x_head_turn = -(ball_x-(320.0 / 2.0)) / 160.0
print('X HEAD TURN: {}'.format(x_head_turn))
commands.setHeadPan(x_head_turn)
y_head_tilt = -(ball_y-(240.0 / 2.0)) / 120.0 * 30
print('Y HEAD TILT: {}'.format(y_head_tilt))
commands.setHeadTilt(y_head_tilt)
else:
memory.speech.say("Where's the ball!?")
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):
commands.setStiffness()
# commands.stand()
ball = mem_objects.world_objects[core.WO_BALL]
selfRobot = mem_objects.world_objects[core.WO_TEAM5]
relBall = ball.loc.globalToRelative(selfRobot.loc, selfRobot.orientation)
if ball.seen:
commands.setHeadPan(ball.bearing, 0.1)
self.lastSeenCt = 0
else:
self.lastSeenCt += 1
if self.lastSeenCt > 120:
commands.setHeadPan(0,0.1)
eta = float('inf')
print "Ball: {0}, {1} Velocity: {2}, {3} Vision: {4} {5}".format(relBall.x, relBall.y, ball.absVel.x, ball.absVel.y, ball.visionDistance, ball.visionBearing*core.RAD_T_DEG)
if ball.absVel.x < 0:
# eta = -1.0 * (ball.loc.x + 1000) / ball.absVel.x
eta = -1.0 * relBall.x / ball.absVel.x
#if abs(ball.loc.x / ball.relVel.x) < 3.0 and ball.relVel.x < 0: # Ball will reach us in 3 seconds
if eta < 10 and relBall.x < 1000 and eta > 3:
# intercept = ball.loc.y + (ball.absVel.y * eta)
intercept = relBall.y + ball.absVel.y * eta
print eta
print ball.loc
print relBall
print ball.absVel
# print ball.relVel
print intercept
if intercept < 500 and intercept > -500:
UTdebug.log(15, "Ball is close, blocking!")
if intercept > 120:
choice = "left"
elif intercept < -120:
choice = "right"
else:
choice = "center"
self.postSignal(choice)
def run(self):
global direction
global angle
global speed
ball = memory.world_objects.getObjPtr(core.WO_BALL)
angle = core.joint_values[core.HeadPan]
speed = 8
normalSpeed = 0.5
commands.setStiffness()
if ball.seen:
commands.setHeadPan(ball.visionBearing, (ball.visionBearing-angle)/speed)
self.finish()
else:
if direction == 1:
commands.setHeadPan(1, (1-angle)/normalSpeed)
print "positive direction"
print "angle %f!" % angle
if angle > 0.9:
print "change to negative direciton"
direction = -1
elif direction == -1:
commands.setHeadPan(-1, (1+angle)/normalSpeed)
print "positive direction"
print "angle %f!" % angle
if angle < -0.9:
print "change to positive direciton"
direction = 1
else:
direction = 1;
