def step(goalx, goaly):
print "in step"
for index, robot in enumerate(robots):
if robot.frontx != -1 and robot.fronty != -1:
imageID = "robot" + str(index)
print imageID
coords = canvas.coords("robot3")
#switched axis from simulation to robot axis, which is 90 dgrees off
velx = goalx - robot.frontx
vely = goaly - robot.fronty
orientation_x = robot.frontx - robot.irl_x
orientation_y = robot.fronty - robot.irl_y
#angle = angle_between((velx, vely), (orientation_x, orientation_y))
angle = math.atan2(vely, velx) - math.atan2(
orientation_y, orientation_x)
print "ANGLE: " + str(math.degrees(-angle))
kiwi = Kiwi()
distFromGoal = abs(math.sqrt((velx)**2 + (vely)**2))
"""print "initial velocity:" + str(velx) + ", " + str(vely)
print "Simulation Position: " + str(coords[0]) + " : " + str(coords[1])
kiwi = Kiwi()
(vx, vy) = (robot.frontx - robot.irl_x, robot.fronty - robot.irl_y)
(ux, uy) = (goalx - robot.irl_x, goaly - robot.irl_y)
mag1 = math.sqrt(vx**2 + vy**2)
mag2 = math.sqrt(ux**2 + uy**2)
#Need to determine quadrants for the angle
angle = math.degrees(math.acos(((vx * ux) + (vy * uy))/(mag1 * mag2)))
#angle = math.degrees(math.atan2(float(velx), float(vely))); #Angle between desired velocity and robot if robot is facing in the zero direction
if math. vely < 0:
angle = -angle
#total_y = orientation_x + velx
#total_x = orientation_y + vely
#angle = math.degrees(math.atan2(float(total_x), float(total_y)))
print "CALC ANGLE: " + str(angle)"""
#convert to holonomic
dutyCycles = convertToDutyCycles(velx, vely, math.degrees(-angle))
kiwi.w1 = dutyCycles[0]
kiwi.w2 = dutyCycles[1]
kiwi.w3 = dutyCycles[2]
#print "Velocity to kiwi duty cycle: " + str(kiwi.w1) + ", " + str(kiwi.w2) + ", " + str(kiwi.w3)
robot.publisher.publish(kiwi)
if (distFromGoal < 50):
print "reached Goal: " + str(index)
else:
x_dist = goalx - robot.irl_x #getAgentPosition(robot.agent)[0]
y_dist = goaly - robot.irl_y #getAgentPosition(robot.agent)[1]
canvas.coords("robot3", robot.irl_x, robot.irl_y)
print "ROBOT IRL: " + str(robot.irl_x) + ", " + str(
robot.irl_y)
canvas.pack()
global afterID
afterID = root.after(50, step, goalx, goaly)
def step(goalx, goaly):
#print "in step: " + str(goalx) + ", " + str(goaly)
sim.doStep()
for index, robot in enumerate(robots):
if robot.agent != -1:
position = sim.getAgentPosition(robot.agent)
agentVelocity = sim.getAgentVelocity(robot.agent)
#print "Agent Velocity in px/sec: " + '{:05.2f}'.format(agentVelocity[0]) + ', ' + '{:05.2f}'.format(agentVelocity[1])
vectorx = goalx - position[0] #simulated vector from goal
vectory = goaly - position[1]
distFromGoal = abs(math.sqrt((vectorx)**2 + (vectory)**2))
kiwi = Kiwi()
"""angle = math.radians(robot.irl_angle)
vx = math.cos(angle)*distFromGoal
vy = math.sin(angle)*distFromGoal"""
vx = 0.06 * agentVelocity[0]
vy = 0.06 * agentVelocity[1]
#print "Velocity in in/sec: " + str(vx) + ", " + str(vy)
#Find Angle
(vx, vy) = (robot.frontx - robot.irl_x, robot.fronty - robot.irl_y)
(ux, uy) = (goalx - robot.irl_x, goaly - robot.irl_y)
mag1 = math.sqrt(vx**2 + vy**2)
mag2 = math.sqrt(ux**2 + uy**2)
angle = math.degrees(
math.acos(((vx * ux) + (vy * uy)) / (mag1 * mag2)))
print "ANGLE: " + str(angle)
#convert to holonomic
dutyCycles = convertToDutyCycles(agentVelocity[0],
agentVelocity[1], angle)
kiwi.w1 = dutyCycles[0]
kiwi.w2 = dutyCycles[1]
kiwi.w3 = dutyCycles[2]
#print "Velocity to kiwi duty cycle: " + str(kiwi.w1) + ", " + str(kiwi.w2) + ", " + str(kiwi.w3)
robot.publisher.publish(kiwi)
if (distFromGoal < 100):
print "reached Goal: " + str(index)
sim.setAgentPrefVelocity(robot.agent, (0, 0))
else:
#initial draw for simulated step
#canvas.move(robot.robotShapeID, vectorx*.005, vectory*.005)
#canvas redraws what opencv sees, COMMENT IF NO CAMERA ATTACHED
#canvas.coords(robot.robotShapeID, robot.irl_x-10, robot.irl_y-10, robot.irl_x+10, robot.irl_y+10)
x_dist = goalx - robot.irl_x #getAgentPosition(robot.agent)[0]
y_dist = goaly - robot.irl_y #getAgentPosition(robot.agent)[1]
sim.setAgentPrefVelocity(robot.agent, (x_dist, y_dist))
sim.setAgentPosition(robot.agent, (robot.irl_x, robot.irl_y))
canvas.coords(robot.robotShapeID, robot.irl_x, robot.irl_y)
print "ROBOT IRL: " + str(robot.irl_x) + ", " + str(
robot.irl_y)
#canvas.coords(robot.robotShapeID, position[0], position[1])
canvas.pack()
global afterID
afterID = root.after(50, step, goalx, goaly)
def step(goalx, goaly):
print "in step"
for index, robot in enumerate(robots):
if robot.frontx != -1 and robot.fronty != -1:
imageID = "robot" + str(index)
print imageID
coords = canvas.coords("robot3")
#switched axis from simulation to robot axis, which is 90 dgrees off
vely = (
(goalx - coords[0]) / 600) * 255 #simulated vector from center
velx = ((goaly - coords[1]) / 800) * 255
print "initial velocity:" + str(velx) + ", " + str(vely)
print "Simulation Position: " + str(coords[0]) + " : " + str(
coords[1])
#velx = math.cos(math.radians(robot.irl_angle))*(velx / 600) * 150
#vely = -math.sin(math.radians(robot.irl_angle)) *(vely / 400) * 150
print "vel:" + str(velx) + ", " + str(vely)
distFromGoal = abs(math.sqrt((velx)**2 + (vely)**2))
kiwi = Kiwi()
(vx, vy) = (robot.frontx - robot.irl_x, robot.fronty - robot.irl_y)
(ux, uy) = (goalx - robot.irl_x, goaly - robot.irl_y)
mag1 = math.sqrt(vx**2 + vy**2)
mag2 = math.sqrt(ux**2 + uy**2)
angle = math.degrees(math.atan2(float(velx), float(vely)))
#angle = math.degrees(math.acos2(((vx * ux) + (vy * uy))/(mag1 * mag2)))
print "X_IRL" + str(robot.irl_x) + ", " + " Y_IRL " + str(
robot.irl_y) + " CALC ANGLE: " + str(angle)
#convert to holonomic
dutyCycles = convertToDutyCycles(velx, vely, angle)
kiwi.w1 = dutyCycles[0]
kiwi.w2 = dutyCycles[1]
kiwi.w3 = dutyCycles[2]
#print "Velocity to kiwi duty cycle: " + str(kiwi.w1) + ", " + str(kiwi.w2) + ", " + str(kiwi.w3)
robot.publisher.publish(kiwi)
if (distFromGoal < 50):
print "reached Goal: " + str(index)
else:
x_dist = goalx - robot.irl_x #getAgentPosition(robot.agent)[0]
y_dist = goaly - robot.irl_y #getAgentPosition(robot.agent)[1]
canvas.coords("robot3", robot.irl_x, robot.irl_y)
print "ROBOT IRL: " + str(robot.irl_x) + ", " + str(
robot.irl_y)
canvas.pack()
global afterID
afterID = root.after(50, step, goalx, goaly)
def step(goalx, goaly):
#print "in step: " + str(goalx) + ", " + str(goaly)
sim.doStep()
for index, robot in enumerate(robots):
if robot.agent != -1:
position = sim.getAgentPosition(robot.agent)
agentVelocity = sim.getAgentVelocity(robot.agent)
print "Agent Velocity in px/sec: " + '{:05.2f}'.format(
agentVelocity[0]) + ', ' + '{:05.2f}'.format(agentVelocity[1])
vectorx = goalx - position[0] #simulated vector from goal
vectory = goaly - position[1]
distFromGoal = abs(math.sqrt((vectorx)**2 + (vectory)**2))
kiwi = Kiwi()
vx = 0.06 * agentVelocity[0]
vy = 0.06 * agentVelocity[1]
print "Velocity in in/sec: " + str(vx) + ", " + str(vy)
#convert to holonomic
dutyCycles = convertToDutyCycles(agentVelocity[0],
agentVelocity[1], robot.irl_angle)
kiwi.w1 = dutyCycles[0]
kiwi.w2 = dutyCycles[1]
kiwi.w3 = dutyCycles[2]
print "Velocity to kiwi duty cycle: " + str(kiwi.w1) + ", " + str(
kiwi.w2) + ", " + str(kiwi.w3)
robot.publisher.publish(kiwi)
if (distFromGoal < 100):
print "reached Goal: " + str(index)
sim.setAgentPrefVelocity(robot.agent, (0, 0))
else:
#initial draw for simulated step
#canvas.move(robot.robotShapeID, vectorx*.005, vectory*.005)
#canvas redraws what opencv sees, COMMENT IF NO CAMERA ATTACHED
x_dist = goalx - robot.irl_x #getAgentPosition(robot.agent)[0]
y_dist = goaly - robot.irl_y #getAgentPosition(robot.agent)[1]
sim.setAgentPrefVelocity(robot.agent, (x_dist, y_dist))
sim.setAgentPosition(robot.agent, (robot.irl_x, robot.irl_y))
#canvas.coords(robot.robotShapeID, robot.irl_x, robot.irl_y)
canvas.coords(robot.robotShapeID, position[0], position[1])
canvas.pack()
global afterID
afterID = root.after(50, step, goalx, goaly)
def cleanCloseProgram():
cleanMsg = Kiwi()
cleanMsg.w1 = cleanMsg.w2 = cleanMsg.w3 = 0
robot0.publisher.publish(cleanMsg)
robot1.publisher.publish(cleanMsg)
robot2.publisher.publish(cleanMsg)
robot3.publisher.publish(cleanMsg)
sys.exit(0)
def step(goalx, goaly):
for index, robotID in enumerate(robots):
coords = canvas.coords(robotID) #SIMULATED perfect position
vectorx = goalx - ((coords[2] - coords[0]) / 2 + coords[0]
) #simulated vector from center
vectory = goaly - ((coords[3] - coords[1]) / 2 + coords[1])
distFromGoal = abs(math.sqrt((vectorx)**2 + (vectory)**2))
#print "vector" + str(vectorx) + ", " + str(vectory)
kiwi = Kiwi()
angle = robot_data[str(index) + "_angle"]
vx = math.cos(angle) * distFromGoal
vy = math.sin(angle) * distFromGoal
#get porportion
vx = vx / 800
vy = vy / 600
kiwi.w1 = vx * 100
#number is the duty cycle %. 100% when vx >
kiwi.w2 = -0.5 * vx - math.sqrt(1.5) * vy * 100
kiwi.w3 = -0.5 * vx + math.sqrt(1.5) * vy * 100
print "kiwi---" + str(kiwi.w1) + ", " + str(kiwi.w2) + ", " + str(
kiwi.w3)
publishers[index].publish(kiwi)
if (distFromGoal < 100):
print "reached Goal: " + str(robotID)
else:
#initial draw for simulated step
canvas.move(robotID, vectorx * .01, vectory * .01)
cur_x = robot_data[str(index) + "_x"]
cur_y = robot_data[str(index) + "_y"]
#canvas redraws what opencv sees, COMMENT IF NO CAMERA ATTACHED
#canvas.coords(robotID, cur_x-10, cur_y-10, cur_x+10, cur_y+10)
canvas.pack()
root.after(200, step, goalx, goaly)
def step(goalx, goaly): #print "in step: " + str(goalstepx) + ", " + str(goalstepy) for index, robot in enumerate(robots): if robot.frontx != -1 and robot.fronty != -1: coords = canvas.coords(robot.robotShapeID) #SIMULATED perfect position velx = goalx - ((coords[2] - coords[0])/2 + coords[0]) #simulated vector from center vely = goaly - ((coords[3] - coords[1])/2 + coords[1]) velx = math.degrees(math.cos((velx / 600) * 150)) vely = math.degrees(math.sin((vely / 400) * 150)) print "vel:" + str(velx) + ", " + str(vely) distFromGoal = abs(math.sqrt((velx)**2 + (vely)**2)) kiwi = Kiwi() (vx, vy) = (robot.frontx - robot.irl_x, robot.fronty - robot.irl_y) (ux, uy) = (goalx - robot.irl_x, goaly - robot.irl_y) mag1 = math.sqrt(vx**2 + vy**2) mag2 = math.sqrt(ux**2 + uy**2) print str(robot.frontx) + ", " + str(robot.fronty) angle = math.degrees(math.acos(((vx * ux) + (vy * uy))/(mag1 * mag2))) print "ANGLE: " + str(angle) #convert to holonomic dutyCycles = convertToDutyCycles(velx, vely, angle) kiwi.w1 = dutyCycles[0] kiwi.w2 = dutyCycles[1] kiwi.w3 = dutyCycles[2] robot.publisher.publish(kiwi) if(distFromGoal < 20): print "reached Goal: " + str(index) else: rotate(robot.robotShapeID, robot.irl_angle) canvas.coords(robot.robotShapeID, robot.irl_x, robot.irl_y, robot.irl_x, robot.irl_y) print "ROBOT IRL: "+ str(robot.irl_x) + ", " + str(robot.irl_y) global afterID afterID = root.after(100, step, goalx, goaly)
def step(goalx, goaly): print "in step: " + str(goalx) + ", " + str(goaly) sim.doStep() for index, robot in enumerate(robots): position = sim.getAgentPosition(robot.agent) vectorx = goalx - position[0] #simulated vector from goal vectory = goaly - position[1] distFromGoal = abs(math.sqrt((vectorx)**2 + (vectory)**2)) kiwi = Kiwi() angle = math.radians(robot.irl_angle) vx = math.cos(angle)*distFromGoal vy = math.sin(angle)*distFromGoal #get porportion vx = vx / 300 vy = vy / 200 kiwi.w1 = -vx * 255; #number is the duty cycle %. 100% when vx > kiwi.w2 = 0.5 * vx - math.sqrt(1.5) * vy * 255; kiwi.w3 = 0.5 * vx + math.sqrt(1.5) * vy * 255; if(kiwi.w1 > 255): kiwi.w1 = 100 elif(kiwi.w1 < -255): kiwi.w1 = 100 if(kiwi.w2 > 255): kiwi.w2 = 100 elif(kiwi.w2 < -255): kiwi.w2 = 100 if(kiwi.w3 > 255): kiwi.w3 = 100 elif(kiwi.w3 < -255): kiwi.w3 = 100 #print "kiwi---" + str(kiwi.w1) + ", " + str(kiwi.w2) + ", " + str(kiwi.w3) robot.publisher.publish(kiwi) if(distFromGoal < 20): print "reached Goal: " + str(index) sim.setAgentPrefVelocity(robot.agent,(0, 0)) else: #initial draw for simulated step #canvas.move(robot.robotShapeID, vectorx*.005, vectory*.005) #canvas redraws what opencv sees, COMMENT IF NO CAMERA ATTACHED #canvas.coords(robot.robotShapeID, robot.irl_x-10, robot.irl_y-10, robot.irl_x+10, robot.irl_y+10) canvas.coords(robot.robotShapeID, position[0]-58, position[1]-58, position[0]+58, position[1]+58) canvas.pack() global afterID afterID = root.after(5, step, goalx, goaly)