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)