def execute(param, state, bot_id): point = Vector2D(param.DribbleTurnP.x, param.DribbleTurnP.y) botPos = Vector2D(state.homePos[bot_id].x, state.homePos[bot_id].y) ballPos = Vector2D(state.ballPos.x, state.ballPos.y) radius = param.DribbleTurnP.turn_radius ob = Vector2D() finalSlope = ob.angle(point, ballPos) turnAngleLeft = ob.normalizeAngle(finalSlope - state.homePos[bot_id].theta) if turnAngleLeft >= -ANGLE_THRES * PI / 180 and turnAngleLeft <= ANGLE_THRES * PI / 180: omega = ((turnAngleLeft) * 180 / (ANGLE_THRES * PI)) * param.DribbleTurnP.max_omega else: omega = param.DribbleTurnP.max_omega * (-1 if turnAngleLeft < 0 else 1) phi = ob.normalizeAngle( atan2(omega * omega * radius, FRICTION_COEFF * ACCN_GRAVITY)) sphi = sin(phi) cphi = cos(phi) if omega >= 0: v_x = omega * radius * sphi v_y = -omega * radius * cphi else: v_x = -omega * radius * sphi v_y = -omega * radius * cphi skill_node.send_command(state.isteamyellow, botID, v_x, v_y, omega, 0, True)
def execute(param, state, bot_id, pub): botPos = Vector2D(int(state.homePos[bot_id].x), int(state.homePos[bot_id].y)) ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y)) destPoint = Vector2D(int(param.KickToPointP.x), int(param.KickToPointP.y)) ob = Vector2D() finalSlope = destPoint.angle(botPos) turnAngleLeft = ob.normalizeAngle( finalSlope - state.homePos[bot_id].theta) #Angle left to turn dist = ballPos.dist(botPos) if dist > BOT_BALL_THRESH: sGoToBall.execute(param, state, bot_id, pub) return if math.fabs(turnAngleLeft ) > SATISFIABLE_THETA / 2: # SATISFIABLE_THETA in config file sParam = skills_union.SParam() sParam.TurnToPointP.x = destPoint.x sParam.TurnToPointP.y = destPoint.y sParam.TurnToPointP.max_omega = MAX_BOT_OMEGA * 3 sTurnToPoint.execute(sParam, state, bot_id, pub) return skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, 0, param.KickToPointP.power, False)
def execute(param, state, bot_id, pub): point = Vector2D(int(param.TurnToPointP.x), int(param.TurnToPointP.y)) botPos = Vector2D(int(state.homePos[bot_id].x), int(state.homePos[bot_id].y)) ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y)) vec = Vector2D() finalslope = point.angle(botPos) turnAngleLeft = vec.normalizeAngle( finalslope - state.homePos[bot_id].theta) #Angle left to turn omega = 2.8 * turnAngleLeft * param.TurnToPointP.max_omega / ( 2 * math.pi) #Speedup turn if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA: if omega < 0: omega = -MIN_BOT_OMEGA else: omega = MIN_BOT_OMEGA v_x = omega * BOT_BALL_THRESH * 1.5 dist = ballPos.dist(botPos) if dist < DRIBBLER_BALL_THRESH * 4: skill_node.send_command(pub, state.isteamyellow, bot_id, v_x, 0, omega, 0, True) else: skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0, False)
def execute(param, state, bot_id, pub): debug(param,state,bot_id) botPos = Vector2D(int(state.homePos[bot_id].x), int(state.homePos[bot_id].y)) ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y)) vec = Vector2D() finalslope = botPos.angle(ballPos) turnAngleLeft = vec.normalizeAngle(finalslope - state.homePos[bot_id].theta) #Angle left to turn omega = 1*(math.pi)*turnAngleLeft *MAX_BOT_OMEGA / (2 * math.pi) #Speedup turn if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA: if omega < 0: omega = -MIN_BOT_OMEGA else: omega = MIN_BOT_OMEGA if turnAngleLeft == 0: omega = 0 v_x = omega * BOT_BALL_THRESH * 1.5 dist = ballPos.dist(botPos) if(fabs(turnAngleLeft)<SATISFIABLE_THETA/2.0): omega=0 if dist < DRIBBLER_BALL_THRESH * 2: skill_node.send_command(pub, state.isteamyellow, bot_id, v_x, 0, omega, 0, True) else: skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0, False)
def execute(param, state, bot_id, pub): print("Testing Motion") speed = param.speed motionAngle = param.motionAngle omega = param.omega theta = state.homePos[bot_id].theta - motionAngle print("Velocity Tangent", speed * math.cos(theta)) print("Velocity Normal", -speed * math.sin(theta)) skill_node.send_command(pub, state.isteamyellow, bot_id, -speed * math.sin(theta), -speed * math.cos(theta), omega, 0, False)
def execute(param, state, bot_id, pub,dribbler = False): t = rospy.Time.now() t = t.secs + 1.0*t.nsecs/pow(10,9) start_time = float(os.environ.get('bot'+str(bot_id))) [vx, vy, vw, REPLANNED, remainingPath] = Get_Vel(start_time, t, bot_id, state.ballPos, state.homePos, state.awayPos) #vx, vy, vw, replanned, remainingPath if(REPLANNED): reset(bot_id) try: skill_node.send_command(pub, False, bot_id, vx, vy, vw, 0,0) except Exception as e: print("In except",e) pass
def execute(param, state, bot_id, pub): point = Vector2D(int(param.GoWithDribbleP.x), int(param.GoWithDribbleP.y)) myPos = Vector2D(int(state.homePos[bot_id].x), int(state.homePos[bot_id].y)) distance = point.dist(myPos) MAX_BOT_SPEED = 0.7 * 1800 speed = 2 * distance * MAX_BOT_SPEED / (HALF_FIELD_MAXX) from math import * theta = myPos.angle(point) print(theta) theta = point.normalizeAngle(theta + state.homePos[bot_id].theta) vel_x = speed * cos(theta) vel_y = speed * sin(theta) skill_node.send_command(pub, state.isteamyellow, bot_id, vel_x, vel_y, 0, 0, True)
def execute(param, state, bot_id, pub): # print "in kicktopoint" botPos = Vector2D(int(state.homePos[bot_id].x), int(state.homePos[bot_id].y)) ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y)) destPoint = Vector2D(int(param.KickToPointP.x), int(param.KickToPointP.y)) ob = Vector2D() finalSlope = destPoint.angle(botPos) turnAngleLeft = ob.normalizeAngle( finalSlope - state.homePos[bot_id].theta) #Angle left to turn ballBotAngle = ob.normalizeAngle( ballPos.angle(botPos) - state.homePos[bot_id].theta) dist = ballPos.dist(botPos) if dist > BOT_BALL_THRESH + 10: print("before kick (GoToBall) dist remaining : ", dist - BOT_BALL_THRESH, " : ", BOT_BALL_THRESH) param.GoToPointP.x = state.ballPos.x param.GoToPointP.y = state.ballPos.y param.GoToPointP.finalSlope = ballPos.angle(state.homePos[bot_id]) sGoToPoint.execute(param, state, bot_id, pub) return # print("After gotoball") if ballBotAngle > SATISFIABLE_THETA / 2: sParam = skills_union.SParam() sParam.TurnToPointP.x = ballPos.x sParam.TurnToPointP.y = ballPos.y sParam.TurnToPointP.max_omega = MAX_BOT_OMEGA print("before kick (TurnToBall) angle remaining : ", ballBotAngle) sTurnToPoint.execute(sParam, state, bot_id, pub) return if math.fabs(turnAngleLeft ) > SATISFIABLE_THETA / 2: # SATISFIABLE_THETA in config file sParam = skills_union.SParam() sParam.TurnToPointP.x = destPoint.x sParam.TurnToPointP.y = destPoint.y sParam.TurnToPointP.max_omega = MAX_BOT_OMEGA print("before kick (Turn) angle remaining : ", turnAngleLeft) sTurnToPoint.execute(sParam, state, bot_id, pub) return # print("after turn") print("____KICK____ DIST :", dist, " ANGLE : ", turnAngleLeft) skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, 0, param.KickToPointP.power, False)
def execute(param, state, bot_id,pub, dribbler=False): debug(param,state,bot_id) finalSlope = param.TurnToAngleP.finalSlope # Yet to be defined ballPos = Vector2D(int(state.ballPos.x),int(state.ballPos.y)) botPos = Vector2D(int(state.homePos[bot_id].x),int(state.homePos[bot_id].y)) obj=Vector2D() turnAngleLeft = obj.normalizeAngle(finalSlope - state.homePos[bot_id].theta); # Angle left to turn omega = 3*turnAngleLeft * MAX_BOT_OMEGA/(2*math.pi); # Speedup turn if(omega < 1.43*MIN_BOT_OMEGA and omega > -1.43*MIN_BOT_OMEGA): # This is a rare used skill so believe in Accuracy more than speed. Hence reducing minimum Omega if(omega < 0): omega = -1.43*MIN_BOT_OMEGA else: omega = 1.43*MIN_BOT_OMEGA dist = ballPos.dist(botPos) if(dist < DRIBBLER_BALL_THRESH): skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0,True) else: skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0,False)
def execute(param, state, bot_id, pub): R = param.DribbleTurnP.turn_radius v = param.DribbleTurnP.max_velocity K = 1000 fp = Vector2D(param.DribbleTurnP.x,param.DribbleTurnP.y) botpointangle = fp.normalizeAngle(fp.angle(state.homePos[bot_id])) print "Bot Pos: {} {}".format(state.homePos[bot_id].x,state.homePos[bot_id].y) print "Bot Point angle: {}".format(fp.angle(state.homePos[bot_id])*180/math.pi) sign = -1 if fp.normalizeAngle(fp.normalizeAngle(state.homePos[bot_id].theta)-fp.normalizeAngle(fp.angle(state.homePos[bot_id]))) >= 0 else 1 print "Sign: {}".format(sign) ta = (v*v)/(K*R) a = sign*abs(math.atan(ta)) print "tananlpha: {} alpha: {}".format(ta,a*180/math.pi) v_y = v*math.cos(a) v_x = v*math.sin(a) w = sign*v/R print "w: {}".format(w) skill_node.send_command(pub, state.isteamyellow, bot_id, v_x, v_y, w, 0, 1)
def execute(param, state, bot_id): finalSlope = param.TurnToAngleP.finalslope # Yet to be defined obj = Vector2D() turnAngleLeft = obj.normalizeAngle(finalSlope - state.homePos[botID].theta) # Angle left to turn omega = turnAngleLeft * MAX_BOT_OMEGA / (2 * math.pi) # Speedup turn if ( omega < MIN_BOT_OMEGA / 2 and omega > -MIN_BOT_OMEGA / 2 ): # This is a rare used skill so believe in Accuracy more than speed. Hence reducing minimum Omega if (omega < 0): omega = -MIN_BOT_OMEGA / 2 else: omega = MIN_BOT_OMEGA / 2 dist = Vector2D(ballPos, botPos) if (dist < DRIBBLER_BALL_THRESH): skill_node.send_command(state.isteamyellow, bot_id, 0, 0, omega, 0, true) else: skill_node.send_command(state.isteamyellow, bot_id, 0, 0, omega, 0, false)
def execute(param, state, bot_id): botPos = Vector2D(state.homePos[bot_id].x, state.homePos[bot_id].y) ballPos = Vector2D(state.ballPos.x, state.ballPos.y) destPoint = Vector2D(param.KickToPointP.x, param.KickToPointP.y) ob = Vector2D() finalSlope = ob.angle(destPoint, botPos) turnAngleLeft = ob.normalizeAngle( finalSlope - state.homePos[bot_id].theta) #Angle left to turn dist = ob.dist(ballPos, botPos) if dist > BOT_BALL_THRESH: sGoToBall.execute(param, state, bot_id) if math.fabs(turnAngleLeft ) > SATISFIABLE_THETA / 2: # SATISFIABLE_THETA in config file paramt = Sparam() paramt.TurnToPointP.x = destPoint.x paramt.TurnToPointP.y = destPoint.y paramt.TurnToPointP.max_omega = MAX_BOT_OMEGA * 3 sTurnToPoint.execute(paramt, state, bot_id) skill_node.send_command(state.isteamyellow, bot_id, 0, 0, 0, param.KickToPointP.power, False)
def execute(param, state, bot_id, gv, pub, dribller=False): # debug(param,state, bot_id) pointPos = Vector2D() pointPos.x = int(param.GoToPointP.x) pointPos.y = int(param.GoToPointP.y) t = rospy.Time.now() t = t.secs + 1.0 * t.nsecs / pow(10, 9) [vx, vy, vw, REPLANNED, maxDisToTurn] = gv[bot_id].execute(t, bot_id, pointPos, state.homePos, state.awayPos) #vx, vy, vw, replanned if (REPLANNED): print("REPLANNED {}".format(bot_id)) reset(bot_id) botPos = Vector2D(int(state.homePos[bot_id].x), int(state.homePos[bot_id].y)) v = Vector2D() distan = botPos.dist(pointPos) ballPos = Vector2D(state.ballPos) # maxDisToTurn = distan if param.GoToPointP.align == True: angleToTurn = pointPos.normalizeAngle(param.GoToPointP.finalSlope - state.homePos[bot_id].theta) else: angleToTurn = pointPos.normalizeAngle( botPos.angle(ballPos) - state.homePos[bot_id].theta) # angleToTurn = v.normalizeAngle((param.GoToPointP.finalSlope)-(state.homePos[bot_id].theta)) minReachTime = maxDisToTurn / MAX_BOT_OMEGA maxReachTime = maxDisToTurn / MIN_BOT_OMEGA minTurnTime = angleToTurn / MAX_BOT_OMEGA maxTurnTime = angleToTurn / MIN_BOT_OMEGA omega = 2 * angleToTurn * MAX_BOT_OMEGA / (2 * math.pi) if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA: if omega < 0: omega = -MIN_BOT_OMEGA else: omega = MIN_BOT_OMEGA if angleToTurn == 0: omega = 0 print("bot id{} vx: {}, vy:{}".format(bot_id, vx, vy)) if param.GoToPointP.align == False: if distan < DRIBBLER_BALL_THRESH: if distan < BOT_BALL_THRESH: skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0, dribller) else: skill_node.send_command(pub, state.isteamyellow, bot_id, vx, vy, omega, 0, dribller) else: skill_node.send_command(pub, state.isteamyellow, bot_id, vx, vy, omega, 0, dribller) else: if distan > BOT_BALL_THRESH: skill_node.send_command(pub, state.isteamyellow, bot_id, vx, vy, omega, 0, dribller) else: skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0, dribller)
def execute(param, state, bot_id, pub, kicking=False): obs = Vector_Obstacle() for i in range(0, len(state.homeDetected)): if state.homeDetected[i] and i != bot_id: o = Obstacle() o.x = state.homePos[i].x o.y = state.homePos[i].y o.radius = 3.3 * BOT_RADIUS obs.push_back(o) for j in range(0, len(state.awayDetected)): if state.awayDetected[j]: o = Obstacle() o.x = state.awayPos[j].x o.y = state.awayPos[j].y o.radius = 3.3 * BOT_RADIUS obs.push_back(o) pointPos = Vector2D() pointPos.x = int(param.GoToPointP.x) pointPos.y = int(param.GoToPointP.y) point = Vector2D() nextWP = Vector2D() nextNWP = Vector2D() pathplanner = MergeSCurve() botPos = Vector2D(int(state.homePos[bot_id].x), int(state.homePos[bot_id].y)) pathplanner.plan(botPos, pointPos, nextWP, nextNWP, obs, len(obs), bot_id, True) v = Vector2D() distan = botPos.dist(pointPos) maxDisToTurn = distan - 5.0 * BOT_BALL_THRESH / 4 angleToTurn = v.normalizeAngle((param.GoToPointP.finalslope) - (state.homePos[bot_id].theta)) minReachTime = maxDisToTurn / MAX_BOT_OMEGA maxReachTime = maxDisToTurn / MIN_BOT_OMEGA minTurnTime = angleToTurn / MAX_BOT_OMEGA maxTurnTime = angleToTurn / MIN_BOT_OMEGA speed = 0.0 omega = angleToTurn * MAX_BOT_OMEGA / (2 * math.pi) if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA: if omega < 0: omega = -MIN_BOT_OMEGA else: omega = MIN_BOT_OMEGA if maxDisToTurn > 0: if minTurnTime > maxReachTime: speed = MIN_BOT_SPEED elif minReachTime > maxTurnTime: speed = MAX_BOT_SPEED elif minReachTime < minTurnTime: speed = maxDisToTurn / minTurnTime elif minTurnTime < minReachTime: speed = MAX_BOT_SPEED else: speed = distan / MAX_FIELD_DIST * MAX_BOT_SPEED vec = Vector2D() motionAngle = nextWP.angle(botPos) theta = motionAngle - state.homePos[bot_id].theta if math.fabs(speed) < 64 * MIN_BOT_SPEED: speed = 64 * MIN_BOT_SPEED if kicking == True: force = 7 else: force = 0 # debug(param, state, botPos, pointPos, nextWP, nextNWP, speed, theta, omega, obs) # print "Motion angle : {}".format(motionAngle) speed = MAX_BOT_SPEED / 1.2 # omega = 0 if param.GoToPointP.align == False: if distan < DRIBBLER_BALL_THRESH: print "here" if distan < 0.9 * BOT_BALL_THRESH: skill_node.send_command(pub, state, bot_id, 0, 0, 0, 0, False) else: skill_node.send_command(pub, state, bot_id, speed * math.sin(-theta), speed * math.cos(-theta), omega, force, False) else: skill_node.send_command(pub, state, bot_id, speed * math.sin(-theta), speed * math.cos(-theta), omega, force, False) else: if distan > BOT_BALL_THRESH / 4: skill_node.send_command(pub, state, bot_id, speed * math.sin(-theta), speed * math.cos(-theta), 0, force, False) else: skill_node.send_command(pub, state, bot_id, 0, 0, 0, force, False)
def execute(param, state, bot_id, pub, dribbler=False): debug(param, state, bot_id) obs = Vector_Obstacle() for i in range(0, len(state.homeDetected)): if state.homeDetected[i] and i != bot_id: o = Obstacle() o.x = state.homePos[i].x o.y = state.homePos[i].y o.radius = 2.5 * BOT_RADIUS obs.push_back(o) for j in range(0, len(state.awayDetected)): if state.awayDetected[j]: o = Obstacle() o.x = state.awayPos[j].x o.y = state.awayPos[j].y o.radius = 2.5 * BOT_RADIUS obs.push_back(o) pointPos = Vector2D() pointPos.x = int(param.GoToPointP.x) pointPos.y = int(param.GoToPointP.y) point = Vector2D() nextWP = Vector2D() nextNWP = Vector2D() pathplanner = MergeSCurve() botPos = Vector2D(int(state.homePos[bot_id].x), int(state.homePos[bot_id].y)) pathplanner.plan(botPos, pointPos, nextWP, nextNWP, obs, len(obs), bot_id, not state.isteamyellow) # Komega = 1.5 # Kvel = 1.7 # netDist = pointPos.dist(state.homePos[bot_id]) # curDist = nextWP.dist(state.homePos[bot_id]) # theta_cov = nextWP.normalizeAngle(nextWP.normalizeAngle(param.GoToPointP.finalSlope) - nextWP.normalizeAngle(state.homePos[bot_id].theta)) # theta_cur = theta_cov * curDist/netDist # v_x = -25*Kvel*curDist*math.sin(theta_cur)/HALF_FIELD_MAXX # v_y = 25*Kvel*curDist*math.cos(theta_cur)/HALF_FIELD_MAXX # omega = Komega*theta_cur # print v_x,",",v_y # skill_node.send_command(pub, state.isteamyellow, bot_id, v_x, v_y, omega, 0, dribbler) dist = pointPos.dist(botPos) maxDisToTurn = dist - DRIBBLER_BALL_THRESH angleToTurn = 0 ballPos = Vector2D(state.ballPos.x, state.ballPos.y) if param.GoToPointP.align == True: angleToTurn = pointPos.normalizeAngle(param.GoToPointP.finalSlope - state.homePos[bot_id].theta) else: angleToTurn = pointPos.normalizeAngle( botPos.angle(ballPos) - state.homePos[bot_id].theta) # angleToTurn = pointPos.normalizeAngle(-math.pi+(pointPos.angle(botPos))-(state.homePos[bot_id].theta)) minReachTime = maxDisToTurn / MAX_BOT_SPEED maxReachTime = maxDisToTurn / MIN_BOT_SPEED minTurnTime = angleToTurn / MAX_BOT_OMEGA maxTurnTime = angleToTurn / MIN_BOT_OMEGA speed = 0.0 omega = angleToTurn * MAX_BOT_OMEGA / (2 * math.pi) if angleToTurn: if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA: if omega < 0: omega = -MIN_BOT_OMEGA else: omega = MIN_BOT_OMEGA else: omega = 0.0 speed = 2 * maxDisToTurn * MAX_BOT_SPEED / (HALF_FIELD_MAXX) if (speed) < 2 * MIN_BOT_SPEED: speed = 2 * MIN_BOT_SPEED if (speed > MAX_BOT_SPEED): speed = MAX_BOT_SPEED vec = Vector2D() motionAngle = nextWP.angle(botPos) theta = -state.homePos[bot_id].theta + motionAngle if dist < DRIBBLER_BALL_THRESH: if dist < 1 * BOT_BALL_THRESH: skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0, True) else: skill_node.send_command(pub, state.isteamyellow, bot_id, speed * math.sin(-theta), speed * math.cos(-theta), omega, 0, True) else: skill_node.send_command(pub, state.isteamyellow, bot_id, speed * math.sin(-theta), speed * math.cos(-theta), omega, 0, False)
def execute(param, state, bot_id, pub, dribller=False): pointPos = Vector2D() pointPos.x = int(state.ballPos.x) pointPos.y = int(state.ballPos.y) botPos = Vector2D(int(state.homePos[bot_id].x), int(state.homePos[bot_id].y)) ballPos = Vector2D(state.ballPos) oppGoal = Vector2D(HALF_FIELD_MAXX, 0) v = Vector2D() targetPoint = Vector2D() # maxDisToTurn = distan if param.GoToBallP.align: angle = param.GoToBallP.finalslope angleToTurn = v.normalizeAngle(param.GoToBallP.finalslope - state.homePos[bot_id].theta) else: angle = ballPos.angle(oppGoal) angleToTurn = v.normalizeAngle( ballPos.angle(oppGoal) - state.homePos[bot_id].theta) # angleToTurn = v.normalizeAngle((param.GoToPointP.finalSlope)-(state.homePos[bot_id].theta)) targetPoint.x = ballPos.x - 2 * DRIBBLER_BALL_THRESH * cos(angle) targetPoint.y = ballPos.y - 2 * DRIBBLER_BALL_THRESH * sin(angle) distan = botPos.dist(targetPoint) t = rospy.Time.now() t = t.secs + 1.0 * t.nsecs / pow(10, 9) start_time = float(os.environ.get('bot' + str(bot_id))) if distan <= DRIBBLER_BALL_THRESH: # print("in DRIBBLER_BALL_THRESH") [vx, vy, vw, REPLANNED, maxDisToTurn] = Get_Vel(start_time, t, bot_id, state.ballPos, state.homePos, state.awayPos, avoid_ball=False) #vx, vy, vw, replanned omega = 2 * angleToTurn * MAX_BOT_OMEGA / (2 * math.pi) if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA: if omega < 0: omega = -MIN_BOT_OMEGA else: omega = MIN_BOT_OMEGA if fabs(angleToTurn) < SATISFIABLE_THETA_SHARP: omega = 0 if distan < BOT_BALL_THRESH: skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0, dribller) else: skill_node.send_command(pub, state.isteamyellow, bot_id, vx, vy, omega, 0, dribller) else: [vx, vy, vw, REPLANNED, maxDisToTurn] = Get_Vel(start_time, t, bot_id, targetPoint, state.homePos, state.awayPos, avoid_ball=True) #vx, vy, vw, replanned omega = 2 * angleToTurn * MAX_BOT_OMEGA / (2 * math.pi) if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA: if omega < 0: omega = -MIN_BOT_OMEGA else: omega = MIN_BOT_OMEGA if fabs(angleToTurn) < SATISFIABLE_THETA_SHARP: omega = 0 skill_node.send_command(pub, state.isteamyellow, bot_id, vx, vy, omega, 0, dribller) if (REPLANNED): reset(bot_id)
def execute(param, state, bot_id): skill_node.send_command(state.isteamyellow, bot_id, 0, 0, 0, param.KickP.power, False)
def execute(param, state, bot_id, pub): debug(param, state, bot_id) obs = Vector_Obstacle() for i in range(0, len(state.homeDetected)): if state.homeDetected[i] and i != bot_id: o = Obstacle() o.x = state.homePos[i].x o.y = state.homePos[i].y o.radius = 3.3 * BOT_RADIUS obs.push_back(o) for j in range(0, len(state.awayDetected)): if state.awayDetected[j]: o = Obstacle() o.x = state.awayPos[j].x o.y = state.awayPos[j].y o.radius = 3.3 * BOT_RADIUS obs.push_back(o) ballfinalpos = Vector2D() ballfinalpos.x = int(state.ballPos.x + (state.ballVel.x) / POINTPREDICTIONFACTOR) ballfinalpos.y = int(state.ballPos.y + (state.ballVel.y) / POINTPREDICTIONFACTOR) point = Vector2D() nextWP = Vector2D() nextNWP = Vector2D() pathplanner = MergeSCurve() botPos = Vector2D(int(state.homePos[bot_id].x), int(state.homePos[bot_id].y)) pathplanner.plan(botPos, ballfinalpos, nextWP, nextNWP, obs, len(obs), bot_id, not state.isteamyellow) ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y)) dist = ballPos.dist(botPos) maxDisToTurn = dist - DRIBBLER_BALL_THRESH angleToTurn = ballPos.normalizeAngle(-math.pi + (ballPos.angle(botPos)) - (state.homePos[bot_id].theta)) minReachTime = maxDisToTurn / MAX_BOT_SPEED maxReachTime = maxDisToTurn / MIN_BOT_SPEED minTurnTime = angleToTurn / MAX_BOT_OMEGA maxTurnTime = angleToTurn / MIN_BOT_OMEGA speed = 0.0 omega = angleToTurn * MAX_BOT_OMEGA / (2 * math.pi) if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA: if omega < 0: omega = -MIN_BOT_OMEGA else: omega = MIN_BOT_OMEGA speed = 2 * maxDisToTurn * MAX_BOT_SPEED / (HALF_FIELD_MAXX) if (speed) < 2 * MIN_BOT_SPEED: speed = 2 * MIN_BOT_SPEED if (speed > MAX_BOT_SPEED): speed = MAX_BOT_SPEED vec = Vector2D() motionAngle = nextWP.angle(botPos) theta = -state.homePos[bot_id].theta + motionAngle if dist < DRIBBLER_BALL_THRESH: if dist < 1 * BOT_BALL_THRESH: skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0, True) else: skill_node.send_command(pub, state.isteamyellow, bot_id, speed * math.sin(-theta), speed * math.cos(-theta), omega, 0, True) else: skill_node.send_command(pub, state.isteamyellow, bot_id, speed * math.sin(-theta), speed * math.cos(-theta), omega, 0, False)
def execute(param, state, bot_id, pub): DEFEND_RADIUS = 50.0 ob = Vector2D() botPos = Vector2D(int(state.homePos[bot_id].x), int(state.homePos[bot_id].y)) point_ball_angle = math.atan2(state.ballPos.y - param.DefendPointP.y, state.ballPos.x - param.DefendPointP.x) dpoint = Vector2D( int(param.DefendPointP.x + (param.DefendPointP.radius) * math.cos(point_ball_angle)), int(param.DefendPointP.y + (param.DefendPointP.radius) * math.sin(point_ball_angle))) obs = Vector_Obstacle() for i in range(0, len(state.homeDetected)): if state.homeDetected[i] and i != bot_id: o = Obstacle() o.x = state.homePos[i].x o.y = state.homePos[i].y o.radius = 3.3 * BOT_RADIUS obs.push_back(o) for j in range(0, len(state.awayDetected)): if state.awayDetected[j]: o = Obstacle() o.x = state.awayPos[j].x o.y = state.awayPos[j].y o.radius = 3.3 * BOT_RADIUS obs.push_back(o) point = Vector2D() nextWP = Vector2D() nextNWP = Vector2D() # calling of pathplanner has to be noticed later pathPlanner = MergeSCurve() pathPlanner.plan(botPos, dpoint, nextWP, nextNWP, obs, len(state.homeDetected) + len(state.awayDetected), bot_id, True) motionAngle = nextWP.angle(botPos) if nextNWP.valid() is True: # this has to be checked finalSlope = nextNWP.angle(nextWP) else: finalSlope = nextWP.angle(botPos) finalSlope = point_ball_angle turnAngleLeft = ob.normalizeAngle(finalSlope - state.homePos[bot_id].theta) omega = turnAngleLeft * MAX_BOT_OMEGA / (2 * math.pi) #Speedup turn if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA: if omega < 0: omega = -MIN_BOT_OMEGA else: omega = MIN_BOT_OMEGA dist = nextWP.dist(botPos) # Distance of next waypoint from the bot theta = motionAngle - state.homePos[ bot_id].theta #Angle of dest with respect to bot's frame of reference profileFactor = (dist * 2 / MAX_FIELD_DIST) * MAX_BOT_SPEED if profileFactor < MIN_BOT_SPEED: profileFactor = MIN_BOT_SPEED elif profileFactor > MAX_BOT_SPEED: profileFactor = MAX_BOT_SPEED if dist < BOT_POINT_THRESH: if (turnAngleLeft) > -DRIBBLER_BALL_ANGLE_RANGE and ( turnAngleLeft) < DRIBBLER_BALL_ANGLE_RANGE: skill_node.send_command( pub, state.isteamyellow, bot_id, 0, 0, 0, 0, True) # previous comment here was kick , needs be checked else: skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0, True) else: skill_node.send_command(pub, state.isteamyellow, bot_id, profileFactor * math.sin(-theta), profileFactor * math.cos(-theta), omega, 0, False)
def execute(param, state, bot_id): obs = [] for i in range(0, len(state.homeDetected)): if state.homeDetected[i]: o = obstacle() o.x = state.homePos[i].x o.y = state.homePos[i].y o.radius = 2.5 * BOT_RADIUS obs.append(o) for j in range(0, len(state.awayDetected)): if state.awayDetected[j]: o = obstacle() o.x = state.awayPos[j].x o.y = state.awayPos[j].y o.radius = 2.5 * BOT_RADIUS obs.append(o) pointPos = Vector2D() pointPos.x = param.GoToPointP.x pointPos.y = param.GoToPointP.y point = Vector2D() nextWP = Vector2D() nextNWP = Vector2D() pathplanner = MergeSCurve() botPos = Vector2D(state.homePos[bot_id].x, state.homePos[bot_id].y) pathplanner.plan(botPos, pointPos, nextWP, nextNWP, obs, len(obs), bot_id, True) v = Vector2D() dist = v.dist(botPos, pointPos) maxDisToTurn = dist - 3.5 * BOT_BALL_THRESH angleToTurn = v.normalizeAngle((param.GoToPointP.finalslope) - (state.homePos[bot_id].theta)) minReachTime = maxDisToTurn / MAX_BOT_OMEGA maxReachTime = maxDisToTurn / MIN_BOT_OMEGA minTurnTime = angleToTurn / MAX_BOT_OMEGA maxTurnTime = angleToTurn / MIN_BOT_OMEGA speed = 0.0 omega = angleToTurn * MAX_BOT_OMEGA / (2 * math.pi) if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA: if omega < 0: omega = -MIN_BOT_OMEGA else: omega = MIN_BOT_OMEGA if maxDisToTurn > 0: if minTurnTime > maxReachTime: speed = MIN_BOT_SPEED elif minReachTime > maxTurnTime: speed = MAX_BOT_SPEED elif minReachTime < minTurnTime: speed = maxDisToTurn / minTurnTime elif minTurnTime < minReachTime: speed = MAX_BOT_SPEED else: speed = dist / MAX_FIELD_DIST * MAX_BOT_SPEED vec = Vector2D() motionAngle = vec.angle(nextWP, botPos) theta = motionAngle - state.homePos[bot_id].theta if param.GoToPointP.align == False: if dist < DRIBBLER_BALL_THRESH: if dist < BOT_BALL_THRESH: skill_node.send_command(bot_id, 0, 0, 0, 0, True) else: skill_node.send_command(bot_id, speed * math.sin(-theta), speed * math.cos(-theta), omega, 0, True) else: skill_node.send_command(bot_id, speed * math.sin(-theta), speed * math.cos(-theta), omega, 0, False) else: if dist > BOT_BALL_THRESH: skill_node.send_command(bot_id, speed * math.sin(-theta), speed * math.cos(-theta), 0, 0, False) else: skill_node.send_command(bot_id, 0, 0, 0, 0, True)
def execute(param,state,bot_id, pub,dribbler = False): obs = Vector_Obstacle() for i in range(0,len(state.homeDetected)): if state.homeDetected[i] and i != bot_id: o = Obstacle() o.x=state.homePos[i].x o.y=state.homePos[i].y o.radius=2.5*BOT_RADIUS obs.push_back(o) for j in range(0,len(state.awayDetected)): if state.awayDetected[j]: o = Obstacle() o.x=state.awayPos[j].x o.y=state.awayPos[j].y o.radius=2.5*BOT_RADIUS obs.push_back(o) pointPos = Vector2D() pointPos.x = int(param.GoToPointP.x) pointPos.y = int(param.GoToPointP.y) point = Vector2D() nextWP = pointPos nextNWP = Vector2D() # pathplanner = MergeSCurve() botPos = Vector2D(int(state.homePos[bot_id].x), int(state.homePos[bot_id].y)) # pathplanner.plan(botPos,pointPos,nextWP,nextNWP,obs,len(obs),bot_id, True) v = Vector2D() distan = botPos.dist(pointPos) maxDisToTurn = distan angleToTurn = v.normalizeAngle((param.GoToPointP.finalSlope)-(state.homePos[bot_id].theta)) minReachTime = maxDisToTurn / MAX_BOT_OMEGA maxReachTime = maxDisToTurn / MIN_BOT_OMEGA minTurnTime = angleToTurn / MAX_BOT_OMEGA maxTurnTime = angleToTurn / MIN_BOT_OMEGA speed = 0.0 omega = 2*angleToTurn * MAX_BOT_OMEGA / (2 * math.pi) speed= 4*maxDisToTurn*MAX_BOT_SPEED/(HALF_FIELD_MAXX) if (speed)< 2*MIN_BOT_SPEED: speed=2*MIN_BOT_SPEED if (speed >= MAX_BOT_SPEED/2.0): speed=MAX_BOT_SPEED # omega = 0 vec = Vector2D() motionAngle = nextWP.angle(botPos) theta = motionAngle - state.homePos[bot_id].theta if distan >= 20 * BALL_RADIUS : speed = MAX_BOT_SPEED skill_node.send_command(pub, state.isteamyellow, bot_id, speed * math.sin(-theta), speed * math.cos(-theta), omega, 0, dribbler)
def execute(param, state, bot_id, pub): skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, 0, 0, False)
def execute(param, state, bot_id, pub): centre = Vector2D(int(param.MoveOnArcP.centrex), int(param.MoveOnArcP.centrey)) finalPoint = Vector2D(int(param.MoveOnArcP.finalx), int(param.MoveOnArcP.finaly)) botPos = Vector2D(int(state.homePos[bot_id].x), int(state.homePos[bot_id].y)) ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y)) print "\n\n\n\n\n\n" print " in MoveOnArc bot no " + str(bot_id) + " got final point " + str( finalPoint.x) + str(finalPoint.y) if (math.fabs(botPos.dist(centre) - finalPoint.dist(centre)) > RADIUS_THRESH): param.GoToPointP.x = finalPoint.x param.GoToPointP.y = finalPoint.y param.GoToPointP.finalslope = ballPos.angle(finalPoint) sGoToPoint.execute(param, state, bot_id, pub) return print "HEEEEEEEEEEEEEEEEEEEEE" Radius = botPos.dist(centre) AngleLeft = botPos.normalizeAngle( finalPoint.angle(centre) - botPos.angle(centre)) print " botPos=" + str(botPos.x) + "," + str(botPos.y) print " finalPoint=" + str(finalPoint.x) + "," + str(finalPoint.y) print " AngleLeft=" + str(AngleLeft * 180 / math.pi) print " Radius=" + str(Radius) print " centre=" + str(centre.x) + "," + str(centre.y) speed = MAX_BOT_SPEED * AngleLeft / (2 * math.pi) if (speed < 16 * MIN_BOT_SPEED): speed = 16 * MIN_BOT_SPEED v_x = -speed * math.sin(botPos.angle(centre)) v_y = +speed * math.cos(botPos.angle(centre)) if (AngleLeft < 0): v_y *= -1 v_x *= -1 theta = state.homePos[bot_id].theta v_oy = v_x * math.cos(theta) + v_y * math.sin(theta) v_ox = v_x * math.sin(theta) - v_y * math.cos(theta) print " v_x=" + str(v_x) + " v_y=" + str(v_y) turnAngleLeft = botPos.normalizeAngle( ballPos.angle(finalPoint) - (state.homePos[bot_id].theta)) # Angle left to turn omega = turnAngleLeft * MAX_BOT_OMEGA / (2 * math.pi) # Speedup turn if ( omega < MIN_BOT_OMEGA / 2 and omega > -MIN_BOT_OMEGA / 2 ): # This is a rare used skill so believe in Accuracy more than speed. Hence reducing minimum Omega if (omega < 0): omega = -MIN_BOT_OMEGA / 2 else: omega = MIN_BOT_OMEGA / 2 #dist = ballPos.dist(botPos) #if(dist < DRIBBLER_BALL_THRESH): #skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0,False) #omega=0 if math.fabs(AngleLeft) < ANGLE_THRES: print "0 vel " skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0, True) else: print "v_ox=" + str(v_ox) + " v_oy=" + str(v_oy) skill_node.send_command(pub, state.isteamyellow, bot_id, v_ox, v_oy, omega, 0, True)
def execute(param, state, bot_id, pub): skill_node.send_command(pub, state.isteamyellow, bot_id, param.VelocityP.v_x, param.VelocityP.v_y, param.VelocityP.v_t, 0, True)
def execute(param, state, bot_id): skill_node.send_command(state.isteamyellow, bot_id, 0, 0, param.SpinP.radPerSec, 0, false)
def execute(param, state, bot_id): point = Vector2D(int(param.DribbleToPointP.x), int(param.DribbleToPointP.y)) botPos = Vector2D(int(state.homePos[botID].x), int(state.homePos[botID].y)) ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y)) ballSlope = ballPos.angle(botPos) ballDist = ballPos.dist(botPos) ballTheta = math.fabs( ballDist.normalizeAngle(state.homePos[botID].theta - ballSlope)) # if(ballDist > BOT_BALL_THRESH || ballTheta > DRIBBLER_BALL_ANGLE_RANGE): # return goToBall(param, state, botID) # No python equivalent found obs = [] j = 0 for i, bot in enumerate(state.homePos): if state.homeDetected[i]: obs.append(Obstacle(bot.x, bot.y, 0, 0, 3 * BOT_RADIUS)) j += 1 for i in state.homePos: if state.homeDetected[i]: x = state.awayPos[i - (j - 1)].x y = state.awayPos[i - (j - 1)].y radius = 3 * BOT_RADIUS o = Obstacle(x, y, 0, 0, radius) obs.append(o) nextWP = Vector2D() nextNWP = Vector2D pathplanner = MergeSCurve() pathplanner.plan(botPos, pointPos, nextWP, nextNWP, obs, len(obs), botID, True) if (nextWP.valid()): pass # comm.addCircle(nextWP.x, nextWP.y, 50) # comm.addLine(state.homePos[botID].x, state.homePos[botID].y, nextWP.x, nextWP.y) #Python equivalent for comm not found if (nextNWP.valid()): pass # comm.addCircle(nextNWP.x, nextNWP.y, 50) # comm.addLine(nextWP.x, nextWP.y, nextNWP.x, nextNWP.y) # rospy.loginfo("Dribbling\n") dist = nextWP.dist(botPos) # angle = nextWP.angle(botPos) angle = param.DribbleToPointP.finalslope theta = nextWP(normalizeAngle(state.homePos[botID].theta - angle)) profileFactor = 2 * dist * MAX_BOT_SPEED / MAX_FIELD_DIST if (math.fabs(profileFactor) < MIN_BOT_SPEED): if (profileFactor < 0): profileFactor = -MIN_BOT_SPEED else: profileFactor = MIN_BOT_SPEED v_y = profileFactor * math.cos(theta) v_x = profileFactor * math.sin(theta) romega = theta / (2 * math.pi) * MAX_BOT_OMEGA if (v_y < -MAX_BACK_DRIBBLE_V_Y): v_x /= (v_y / -MAX_BACK_DRIBBLE_V_Y) v_y = -MAX_BACK_DRIBBLE_V_Y elif (v_y > MAX_FRONT_DRIBBLE_V_Y): v_x /= (v_y / MAX_FRONT_DRIBBLE_V_Y) v_y = MAX_FRONT_DRIBBLE_V_Y if (v_x > MAX_DRIBBLE_V_X): v_y /= (v_x / MAX_DRIBBLE_V_X) v_x = MAX_DRIBBLE_V_X elif (v_x < -MAX_DRIBBLE_V_X): v_y /= (v_x / -MAX_DRIBBLE_V_X) v_x = -MAX_DRIBBLE_V_X if (math.fabs(theta) > SATISFIABLE_THETA): if (math.fabs(romega < MIN_BOT_OMEGA)): if (romega > 0): romega = MIN_BOT_OMEGA else: romega = -MIN_BOT_OMEGA elif (fabs(romega) > MAX_DRIBBLE_R): if (romega > 0): romega = MAX_DRIBBLE_R else: romega = -MAX_DRIBBLE_R else: romega = 0 if (dist < BOT_BALL_THRESH): skill_node.send_command(state.isteamyellow, bot_id, 0, 0, 0, 0, True) else: skill_node.send_command(state.isteamyellow, bot_id, v_x, v_y, -romega, 0, True)
def execute(param, state, bot_id, pub, dribller=False): obs = Vector_Obstacle() for i in range(0, len(state.homeDetected)): if state.homeDetected[i] and i != bot_id: o = Obstacle() o.x = state.homePos[i].x o.y = state.homePos[i].y o.radius = 3.3 * BOT_RADIUS obs.push_back(o) for j in range(0, len(state.awayDetected)): if state.awayDetected[j]: o = Obstacle() o.x = state.awayPos[j].x o.y = state.awayPos[j].y o.radius = 3.3 * BOT_RADIUS obs.push_back(o) pointPos = Vector2D() pointPos.x = int(param.GoToPointP.x) pointPos.y = int(param.GoToPointP.y) point = Vector2D() nextWP = Vector2D() nextNWP = Vector2D() pathplanner = MergeSCurve() botPos = Vector2D(int(state.homePos[bot_id].x), int(state.homePos[bot_id].y)) pathplanner.plan(botPos, pointPos, nextWP, nextNWP, obs, len(obs), bot_id, True) v = Vector2D() distan = botPos.dist(pointPos) maxDisToTurn = distan angleToTurn = v.normalizeAngle((param.GoToPointP.finalslope) - (state.homePos[bot_id].theta)) minReachTime = maxDisToTurn / MAX_BOT_OMEGA maxReachTime = maxDisToTurn / MIN_BOT_OMEGA minTurnTime = angleToTurn / MAX_BOT_OMEGA maxTurnTime = angleToTurn / MIN_BOT_OMEGA speed = 0.0 omega = 2 * angleToTurn * MAX_BOT_OMEGA / (2 * math.pi) if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA: if omega < 0: omega = -MIN_BOT_OMEGA else: omega = MIN_BOT_OMEGA from math import exp speed = 2 * maxDisToTurn * MAX_BOT_SPEED / (HALF_FIELD_MAXX) if (speed) < 2 * MIN_BOT_SPEED: speed = 2 * MIN_BOT_SPEED if (speed > MAX_BOT_SPEED): speed = MAX_BOT_SPEED vec = Vector2D() motionAngle = nextWP.angle(botPos) theta = motionAngle - state.homePos[bot_id].theta if param.GoToPointP.align == False: if distan < DRIBBLER_BALL_THRESH: if distan < BOT_BALL_THRESH: skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0, dribller) else: skill_node.send_command(pub, state.isteamyellow, bot_id, speed * math.sin(-theta), speed * math.cos(-theta), omega, 0, dribller) else: skill_node.send_command(pub, state.isteamyellow, bot_id, speed * math.sin(-theta), speed * math.cos(-theta), omega, 0, dribller) else: if distan > BOT_BALL_THRESH: skill_node.send_command(pub, state.isteamyellow, bot_id, speed * math.sin(-theta), speed * math.cos(-theta), omega, 0, dribller) else: skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0, dribller)
import skill_node import maths import sGoToBall import sTurnToPoint def execute(param, state, bot_id): botPos=Vector2D(state.homePos[bot_id].x, state.homePos[bot_id].y) ballPos=Vector2D(state.ballPos.x, state.ballPos.y) destPoint=Vector2D(param.KickToPointP.x, param.KickToPointP.y) ob = Vector2D() finalSlope = ob.angle(destPoint, botPos) turnAngleLeft = ob.normalizeAngle(finalSlope - state.homePos[bot_id].theta) #Angle left to turn dist = ob.dist(ballPos, botPos) if dist > BOT_BALL_THRESH : return sGoToBall.execute(param,state,bot_id) if math.fabs(turnAngleLeft) > SATISFIABLE_THETA/2 : # SATISFIABLE_THETA in config file Sparam paramt() paramt.TurnToPointP.x = destPoint.x paramt.TurnToPointP.y = destPoint.y paramt.TurnToPointP.max_omega = MAX_BOT_OMEGA*3 return sTurnToPoint.execute(paramt, state, bot_id) skill_node.send_command(state.isteamyellow, bot_id ,0, 0, 0, param.KickToPointP.power, False)