if __name__=="__main__": #parser = argparse.ArgumentParser(description="Test Sphero location") #args, unknown = parser.parse_known_args() viconStartNumHere = int(sys.argv[-3]) viconBegin = int(sys.argv[-2]) viconEnd = int(sys.argv[-1]) Diff = viconStartNumHere-1 numAll = viconEnd-viconBegin+1 Num = len(sys.argv)-4 sphero_name = [[]]*Num for i in range(Num): sphero_name[i] = 'sphero_'+str(sys.argv[i+1]) obj = swarmParams.sysParams(numAll) odd = -1 theta0 = math.pi/6 radius0 = 2.0 waypoints = [[]]*Num for i in range(Num): waypoints[i] = odd*radius0*numpy.array([ [math.cos(i*2*math.pi/Num+theta0),math.sin(i*2*math.pi/Num+theta0)] ]) # neighborRadius = 10.0 size_waypoints=waypoints[0].shape #[m,n] length and width of waypoints
#loc_goal=[] #loc_goal.append(1) #loc_goal.append(1) if __name__ == "__main__": #parser = argparse.ArgumentParser(description="Test Sphero location") #args, unknown = parser.parse_known_args() sphero_name1 = str(sys.argv[1]) sphero_name2 = str(sys.argv[2]) sphero_name3 = str(sys.argv[3]) sphero_name4 = str(sys.argv[4]) Num = int(sys.argv[5]) obj = swarmParams.sysParams(8) odd = -1 waypoints1 = odd * numpy.array([[-1.5, 0.0]]) waypoints2 = odd * numpy.array([[0.0, -1.5]]) waypoints3 = odd * numpy.array([[1.5, 0.0]]) waypoints4 = odd * numpy.array([[0.0, 1.5]]) waypoints = [waypoints1, waypoints2, waypoints3, waypoints4] neighborRadius = 10.0 size_waypoints = waypoints[0].shape #[m,n] length and width of waypoints num_waypoints = size_waypoints[ 0] #first entry is length (width is 2-(x,y))