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):
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):
commands.stand()
commands.setHeadPanTilt(0.0, 0.0, 1.5)
if self.getTime() > 4.5:
location = memory.planning.getDestPose()
# print("[%f,%f,%f]" % (location.translation.x, location.translation.y, location.rotation))
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 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):
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):
commands.setHeadPanTilt(pan=x_diff,
tilt=y_diff,
time=DELAY,
isChange=True)
if self.getTime() > DELAY:
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, -elevation, 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
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 abs(distance) >= 600.0:
commands.setWalkVelocity(1.0, 0.0, theta_cont)
else:
commands.setWalkVelocity(dist_cont, 0.0, theta_cont)
self.theta_prev = bearing
self.dist_prev = distance
self.time_last = self.time_current
# print("bearing: %f, distance: %f"%(self.ball.bearing, self.ball.distance))
if abs(distance - self.dist) < 200.0:
self.finish()
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 turning_counter, head_bearing, head_move_side
if head_bearing < -2:
head_move_side = 1
elif head_bearing > 2:
head_move_side = -1
head_bearing += head_move_side * 0.1
turning_counter = 0
commands.setHeadPanTilt(pan=head_bearing, tilt=0, time=0.1)
commands.setWalkVelocity(vx, vy, vtheta)
def run(self):
commands.setHeadPanTilt(0.0, -10.0, 1.0)
robot = world_objects.getObjPtr(core.WO_TEAM5)
self.checkLocalized(robot)
if self.localized:
print("Localized")
self.postSignal("localized")
return
else:
print("Lost")
self.postSignal("lost")
return
def run(self):
turn_head = self.Turn_Head()
ball = memory.world_objects.getObjPtr(core.WO_BALL)
off = self.Off()
stand = self.Stand()
print("Ball seen: ",ball.seen)
if ball.seen:
print("Inside first loop\n")
print(ball.visionBearing,ball.visionElevation,ball.imageCenterX,ball.imageCenterY)
if ball.imageCenterX!=0:
print("ball seen in PYTHON")
turn_head = self.Turn_Head()
commands.setHeadPanTilt(ball.visionBearing,ball.visionElevation*10)
# commands.setHeadPanTilt(-1,20)
print("Set")
def run(self):
self.time_current = time.clock()
dt = self.time_current - self.time_last
self.calc_integral(dt)
bearing = self.ball.visionBearing
distance = self.ball.visionDistance
elevation = core.RAD_T_DEG * self.ball.visionElevation
commands.setHeadPanTilt(bearing,-45.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
commands.setWalkVelocity(dist_cont, 0.6, theta_cont)
self.theta_prev = bearing
self.dist_prev = distance
self.time_last = self.time_current
if self.getTime() > 2.0:
self.finish()
def run(self):
# The desired location on the field
self.destloc = memory.planning.getDestPose()
self.tk = time.clock()
dt = self.tk - self.tkm1
x_des = self.destloc.translation.x
y_des = self.destloc.translation.y
t_des = np.arctan2(y_des - self.robot.loc.y, x_des - self.robot.loc.x)
e_t = t_des - self.robot.orientation
if np.sign(t_des) * e_t - np.pi > 0:
e_t = e_t - np.sign(t_des) * 2 * np.pi
self.calc_int(e_t, dt)
commands.setHeadPanTilt(e_t, 0.0, 1.5)
de_t = (e_t - self.t_prev)
# print("pathIdx: %f. Current Loc: [x: %f, y: %f, t:%f] (%d, %d)" % (memory.planning.pathIdx, self.robot.loc.x, self.robot.loc.y, core.RAD_T_DEG * self.robot.orientation, memory.planning.getGridRowFromLoc(self.robot.loc.y), memory.planning.getGridColFromLoc(self.robot.loc.x)))
# print("destloc: [%f,%f,%f] (%d, %d)" % (self.destloc.translation.x, self.destloc.translation.y, core.RAD_T_DEG * self.destloc.rotation, memory.planning.getDestGridRow(), memory.planning.getDestGridCol()))
# Bound the derivative term of theta
if dt == 0:
de_t = 0.0
else:
de_t = de_t / dt
t_cont = self.k_t[0] * e_t + self.k_t[1] * self.t_int + self.k_t[
2] * de_t
# print("t_cont = %f" % (t_cont))
if abs(e_t) >= 0.3:
commands.setWalkVelocity(0.0, 0.0, 0.4 * np.sign(e_t))
else:
commands.setWalkVelocity(0.0, 0.0, t_cont)
self.t_prev = e_t
self.tkm1 = self.tk
if (abs(e_t) < 0.1):
memory.planning.observedNextGC = True
self.finish()
def run(self):
commands.stand()
commands.setHeadPanTilt(0.0, 0.0, 1.0)
print("ball x: %f, ball y: %f, goal x: %f, goal y: %f" %
(self.ball.loc.x, self.ball.loc.y, self.goal.loc.x,
self.goal.loc.y))
def run(self):
commands.setHeadPanTilt(math.pi/2,-30.0,1.5)
commands.setHeadTilt(-30.0)
if self.getTime() > 2.5:
self.finish()
def run(self):
ball = memory.world_objects.getObjPtr(core.WO_BALL)
bearing = ball.visionBearing
elevation = ball.visionElevation
print('Ball!\t Bearing: %f \t Distance: %f\t Elevation: %.8f\t Location: [%d,%d]\n' % (ball.visionBearing, ball.visionDistance, ball.visionElevation,ball.imageCenterX, ball.imageCenterY))
commands.setHeadPanTilt(bearing, -30.0, 0.1)
def run(self):
commands.setHeadPanTilt(pan=x_diff, tilt=y_diff, time=0.5, isChange=True)
def run(self):
commands.setHeadPanTilt(2,20)
self.postSignal("done")
def run(self):
commands.stand()
commands.setHeadPanTilt(pan=0, tilt=0, time=0.5, isChange=True)
if self.getTime() > 3.0:
memory.speech.say("stand up")
self.finish()
def run(self):
commands.stand()
commands.setHeadPanTilt(0.0, -5.0, 1.0)
if self.getTime() > 1.5:
self.finish()
def run(self):
# print("STAND????")
commands.stand()
commands.setHeadPanTilt(0.0, 0.0, 1.5)
def run(self):
commands.setStiffness()
commands.setHeadPanTilt(pan=0, tilt=-23, time=0.5)
def run(self):
commands.stand()
commands.setHeadPanTilt(0.0, 0.0, self.time)
if self.getTime() > (self.time + 0.3):
self.finish()
def run(self):
commands.setHeadPanTilt(0.0, 0.0, 1.5)
# The desired location on the field
self.destloc = memory.planning.getDestPose()
self.pathidx = memory.planning.pathIdx
th = self.robot.orientation
# The desired x, y, t
x_des = self.destloc.translation.x
y_des = self.destloc.translation.y
t_des = self.destloc.rotation
# print("[pathIdx: %f, robot x: %f, robot y: %f, robot orientation: %f]" % (self.pathidx, self.robot.loc.x, self.robot.loc.y, core.RAD_T_DEG * self.robot.orientation))
# print("destloc: [%f,%f,%f]" % (self.destloc.translation.x, self.destloc.translation.y, core.RAD_T_DEG * self.destloc.rotation))
# The error in x, y, t
e_x = -(self.robot.loc.x - x_des)
e_y = -(self.robot.loc.y - y_des)
e_t = t_des - self.robot.orientation
if np.sign(t_des) * e_t - np.pi > 0:
e_t = e_t - np.sign(t_des) * 2 * np.pi
# if t_des > 0:
# if self.robot.orientation > t_des - np.sign(t_des)*np.pi:
# e_t = t_des - self.robot.orientation
# else:
# e_t = t_des - (2*np.pi + self.robot.orientation)
# else:
# if self.robot.orientation > t_des - np.sign(t_des)*np.pi:
# e_t = t_des - (self.robot.orientation -2*np.pi)
# else:
# e_t = t_des - self.robot.orientation
self.tk = time.clock()
dt = self.tk - self.tkm1
self.calc_int(x_des, y_des, t_des, dt)
de_x = (e_x - self.x_prev)
de_y = (e_y - self.y_prev)
de_t = (e_t - self.t_prev)
# Bound the derivative term of x
if dt == 0 or (self.k_x[2] * de_x > 0.1 * dt):
de_x = 0.0
else:
de_x = de_x / dt
# Bound the derivative term of y
if dt == 0 or (self.k_y[2] * de_y > 0.1 * dt):
de_y = 0.0
else:
de_y = de_y / dt
# Bound the derivative term of theta
if dt == 0 or (self.k_t[2] * de_t > 0.1 * dt):
de_t = 0.0
else:
de_t = de_t / dt
x_cont = self.k_x[0] * e_x + self.k_x[1] * self.x_int + self.k_x[
2] * de_x
# print("[x_cont: %f, ex: %f, kpx: %f, kix: %f, kdx: %f]" % (x_cont, e_x, self.k_x[0] * e_x, self.k_x[1] * self.x_int, self.k_x[2] * de_x))
y_cont = self.k_y[0] * e_y + self.k_y[1] * self.y_int + self.k_y[
2] * de_y
# print("[y_cont: %f, ey: %f, kpy: %f, kiy: %f, kdy: %f]" % (y_cont, e_y, self.k_y[0] * e_y, self.k_y[1] * self.y_int, self.k_y[2] * de_y))
t_cont = self.k_t[0] * e_t + self.k_t[1] * self.t_int + self.k_t[
2] * de_t
# print("[t_cont: %f, et: %f, kpt: %f, kit: %f, kdt: %f]" % (t_cont, e_t, self.k_t[0] * e_t, self.k_t[1] * self.t_int, self.k_t[2] * de_t))
K = np.array([[np.cos(th), np.sin(th), 0],
[-np.sin(th), np.cos(th), 0], [0, 0, 1]])
dstate = np.array([[x_cont], [y_cont], [t_cont]])
u = np.dot(K, dstate)
# print("[u_1: %f, u_2: %f, u_3: %f]" % (u[0], u[1], u[2]))
commands.setWalkVelocity(u[0, 0], u[1, 0], u[2, 0])
for joint in self.listy_thing:
if memory.sensors.getJointTemperature(joint) >= 85.0:
self.postSignal("hot")
self.x_prev = e_x
self.y_prev = e_y
self.t_prev = e_t
self.tkm1 = self.tk
if not (self.id_prev == self.pathidx):
self.postSignal("head")
self.id_prev = self.pathidx
if memory.planning.nodesLeft == 0:
self.finish()
def run(self):
global turning_counter
turning_counter = 0
commands.setHeadPanTilt(pan=head_bearing, tilt=0, time=0.1)
commands.setWalkVelocity(vx, vy, vtheta)
def run(self):
commands.setHeadPanTilt(1.0, 50, 2.0)
def run(self):
commands.stand()
commands.setHeadPanTilt(0.0, 0.0, 1.5)
if self.getTime() > 2.0:
commands.setWalkVelocity(0.0, 0.0, 0.0)
self.finish()
def run(self):
commands.setHeadPanTilt(0, -21, 2.0)
def run(self):
commands.setWalkVelocity(0.1, 0.0, 0.2)
commands.setHeadPanTilt(pan=head_bearing, tilt=0, time=0.1)
