def main():
"""
Robot will use two corners on one side of the wall in the arena.
It will try to put all 4 tokens into slots. This gives us 9 points.
"""
option = 2
robot = Robot()
reset_log(robot)
robot.sound = Sound(robot, USING_SOUND)
robot.sound.play('R2D2')
robot.position = Tracker(robot.zone)
set_pins(robot)
slots_x = 2.91 if robot.zone in [0, 3] else 5.09
target_theta = pi / 2 if robot.zone in [0, 3] else 1.5 * pi
if robot.zone in [0, 1]:
dy = 0.9
slot_y_0 = 2.65
else:
dy = -0.9
slot_y_0 = 5.65
while 1:
try:
if option == 1:
put_down(robot)
grab(robot)
token_to_slot(robot, robot.zone)
for i in range(4):
zone = robot.zone if i < 2 else 3 - robot.zone
has_token = get_token_from_corner(robot, zone)
if has_token:
token_to_slot(robot, zone)
elif option == 2:
put_down(robot)
grab(robot)
for i in range(4):
zone = robot.zone if i < 2 else 3 - robot.zone
if i == 0:
move_to_point(robot, slots_x, slot_y_0 + 0.9,
target_theta)
token_to_slot_2(robot)
elif i == 1:
move_to_point(robot, 2, 2)
get_token_from_corner(robot, zone)
move_to_point(robot, slots_x, slot_y_0, target_theta)
token_to_slot_2(robot)
else:
move_to_point(robot, 2, 2)
get_token_from_corner(robot, zone)
slot_y = slot_y_0 + dy * i
move_to_point(robot, slots_x, slot_y, target_theta)
token_to_slot_2(robot)
except:
print_exc()
restart(robot)
def main():
"""
Robot will use two corners on one side of the wall in the arena.
It will try to put all 4 tokens into slots. This gives us 9 points.
"""
option = 2
robot = Robot()
reset_log(robot)
robot.sound = Sound(robot, USING_SOUND)
robot.sound.play('R2D2')
robot.position = Tracker(robot.zone)
set_pins(robot)
slots_x = 2.91 if robot.zone in [0, 3] else 5.09
target_theta = pi/2 if robot.zone in [0, 3] else 1.5*pi
if robot.zone in [0, 1]:
dy = 0.9
slot_y_0 = 2.65
else:
dy = -0.9
slot_y_0 = 5.65
while 1:
try:
if option == 1:
put_down(robot)
grab(robot)
token_to_slot(robot, robot.zone)
for i in range(4):
zone = robot.zone if i < 2 else 3-robot.zone
has_token = get_token_from_corner(robot, zone)
if has_token:
token_to_slot(robot, zone)
elif option == 2:
put_down(robot)
grab(robot)
for i in range(4):
zone = robot.zone if i < 2 else 3-robot.zone
if i == 0:
move_to_point(robot, slots_x, slot_y_0+0.9, target_theta)
token_to_slot_2(robot)
elif i == 1:
move_to_point(robot, 2, 2)
get_token_from_corner(robot, zone)
move_to_point(robot, slots_x, slot_y_0, target_theta)
token_to_slot_2(robot)
else:
move_to_point(robot, 2, 2)
get_token_from_corner(robot, zone)
slot_y = slot_y_0 + dy*i
move_to_point(robot, slots_x, slot_y, target_theta)
token_to_slot_2(robot)
except:
print_exc()
restart(robot)
def main_test(robot):
while True:
robot.power.led[0] = 1
grab(robot)
robot.power.led[0] = 0
sleep(2)
robot.power.led[1] = 1
put_down(robot)
robot.power.led[1] = 0
sleep(2)
print "Cycled!"
def main_test(robot):
while True:
robot.power.led[0] = 1
grab(robot)
robot.power.led[0] = 0
sleep(2)
robot.power.led[1] = 1
put_down(robot)
robot.power.led[1] = 0
sleep(2)
print "Cycled!"
def get_token_from_corner(robot, zone):
"""
Moves to specified corner, finds a marker and picks it up.
"""
log(robot, "Attempting to get token from corner of zone %d..." % (zone))
token_marker = look_for_token(robot, zone)
if token_marker:
line_up_to_marker(robot, token_marker)
robot.sound.play('Heart')
put_down(robot)
move_till_touch(robot)
grab(robot)
robot.sound.stop()
return True
else:
log(robot, "No tokens found.")
return False
def get_token_from_corner(robot, zone):
"""
Moves to specified corner, finds a marker and picks it up.
"""
log(robot, "Attempting to get token from corner of zone %d..." % (zone))
token_marker = look_for_token(robot, zone)
if token_marker:
line_up_to_marker(robot, token_marker)
robot.sound.play('Heart')
put_down(robot)
move_till_touch(robot)
grab(robot)
robot.sound.stop()
return True
else:
log(robot, "No tokens found.")
return False
def token_to_slot(robot, slot):
"""
Moves robot near the slot, and places token on shelf.
"""
log(robot, "Moving to zone start point...")
slot_x = SLOT_POINTS[slot][0]
slot_y = SLOT_POINTS[slot][1]
slot_theta = 3 * pi / 2 if slot in [1, 2] else pi / 2
move_to_point(robot, slot_x, slot_y, slot_theta)
log(robot, "Scanning for slot markers...")
robot.sound.play('Radar')
markers = robot.see(res=RESOLUTION)
if not markers:
for i in range(3):
markers = robot.see(res=RESOLUTION)
if markers: break
found_Marker = False
for marker in markers:
if marker.info.code in range(32, 40):
log(robot, "Found Token Marker:" + str(marker.info.code))
found_Marker = True
robot.sound.stop()
line_up_to_marker(robot, marker, 0.4)
sleep(0.2)
put_down(robot)
sleep(0.4)
break
if not found_Marker:
robot.sound.stop()
log(robot, "Marker Not Detected.")
move_straight(robot, 0.4)
put_down(robot)
sleep(0.4)
log(robot, "Moving away from marker.")
move_straight(robot, -0.5)
grab(robot)
def token_to_slot(robot, slot):
"""
Moves robot near the slot, and places token on shelf.
"""
log(robot, "Moving to zone start point...")
slot_x = SLOT_POINTS[slot][0]
slot_y = SLOT_POINTS[slot][1]
slot_theta = 3 * pi/2 if slot in [1, 2] else pi/2
move_to_point(robot, slot_x, slot_y, slot_theta)
log(robot, "Scanning for slot markers...")
robot.sound.play('Radar')
markers = robot.see(res=RESOLUTION)
if not markers:
for i in range(3):
markers = robot.see(res=RESOLUTION)
if markers: break
found_Marker = False
for marker in markers:
if marker.info.code in range(32, 40):
log(robot, "Found Token Marker:" + str(marker.info.code))
found_Marker = True
robot.sound.stop()
line_up_to_marker(robot, marker, 0.4)
sleep(0.2)
put_down(robot)
sleep(0.4)
break
if not found_Marker:
robot.sound.stop()
log(robot, "Marker Not Detected.")
move_straight(robot, 0.4)
put_down(robot)
sleep(0.4)
log(robot, "Moving away from marker.")
move_straight(robot, -0.5)
grab(robot)
motionPath = "UMDSpotMiniWalk.txt"
mocapData.Load(motionPath)
print("mocapData.NumFrames=", mocapData.NumFrames())
print("mocapData.KeyFrameDuraction=", mocapData.KeyFrameDuraction())
print("mocapData.getCycleTime=", mocapData.getCycleTime())
print("mocapData.computeCycleOffset=", mocapData.computeCycleOffset())
stablePD = pd_controller_stable.PDControllerStable(p)
cycleTime = mocapData.getCycleTime()
movements.sitDown(p, timeStep, maxForceId, quadruped, jointIds,
jointDirections, jointOffsets)
movements.grab(p, timeStep, maxForceId, quadruped, jointIds, jointDirections,
jointOffsets)
movements.standUp(p, timeStep, maxForceId, quadruped, jointIds,
jointDirections, jointOffsets)
startPosD, startOriD = p.getBasePositionAndOrientation(dumbell)
pos, ori = p.getBasePositionAndOrientation(quadruped)
posd, orid = p.getBasePositionAndOrientation(dumbell)
tempd = []
diffd = []
diffd.append(posd[0] - pos[0])
diffd.append(posd[1] - pos[1])
diffd.append(posd[2] - pos[2])
tempd.append(startPosD[0])
tempd.append(posd[1])
tempd.append(posd[2])