def main():
pub = rospy.Publisher('end_efffector_angles_sub', high_level, queue_size=1)
pi = np.pi
legs = 1.03
body = .5
array = [[0, 0],
[pi*3/4+legs, 0],#right
[pi*5/12, 0],#left
[pi*5/12, pi],#down
[0, pi],#left
[pi*3/2+legs, pi],#right bottom E ###########
[pi, pi],#left
[pi, pi*1/2-body],#up, first half E @@@@@@@
[pi*3/2+legs, pi*1/2],#out first leg ##########
[pi, pi*1/2],#in
[pi, 0],#top of E
[pi*9/4+legs, 0],#top of second E ########
[pi*7/4, 0],#back
[pi*7/4, pi*1/2+body],#down half 2 E @@@@@@@@@@
[pi*9/4+legs, pi*1/2],#out leg ################
[pi*7/4, pi*1/2],#back in
[pi*7/4, pi],#bottom E
[pi*3+legs, pi],#right to bottom 3 E ####
[pi*5/2, pi],#back
[pi*5/2, pi*1/2-body],#up half 3 E @@@@@@@@@@@@@
[pi*3+legs, pi*1/2],#out leg ################
[pi*5/2, pi*1/2],#back in
[pi*5/2, 0],#up top E
[pi*3+legs, 0]]#END ################
for i in range(len(array)):
pub.publish(high_level(large_radians=array[i][0]/2-1.04, small_radians=array[i][1]/2-1.04))
print array[i][0],array[i][1]
time.sleep(.5)
return True
self.dx = (val / (3 * np.pi / 2)) * 200
def right(self, val):
self.dy = (val / (3 * np.pi / 2)) * 200
if __name__ == '__main__':
screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT))
clock = pygame.time.Clock()
my_renderer = Renderer()
pub = rospy.Publisher('end_efffector_angles_sub', high_level)
start_L, start_R = -1.04, -1.04
# start_L, start_R = 0.0, 0.0
pub.publish(high_level(large_radians=start_L, small_radians=start_R))
pt = np.array([0.0, 0.0])
while not rospy.is_shutdown():
for event in pygame.event.get():
if event.type == pygame.QUIT:
exit()
if event.type == pygame.KEYDOWN:
if (event.key == pygame.K_ESCAPE) or (event.key == pygame.K_q):
exit()
if event.type == pygame.MOUSEBUTTONDOWN:
mpos = pygame.mouse.get_pos()
assert max(mpos) < 200
delta = ((mpos - pt) / 200) * (3 * np.pi / 2)
def right(self, val):
self.dy = (val / (3 * np.pi / 2)) * 200
if __name__ == '__main__':
screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT))
clock = pygame.time.Clock()
my_renderer = Renderer()
pub = rospy.Publisher('end_efffector_angles_sub', high_level)
start_L, start_R = -1.04, -1.04
# start_L, start_R = 0.0, 0.0
pub.publish(high_level(large_radians=start_L, small_radians=start_R))
pt = np.array([0.0, 0.0])
while not rospy.is_shutdown():
for event in pygame.event.get():
if event.type == pygame.QUIT:
exit()
if event.type == pygame.KEYDOWN:
if (event.key == pygame.K_ESCAPE) or (event.key == pygame.K_q):
exit()
if event.type == pygame.MOUSEBUTTONDOWN:
mpos = pygame.mouse.get_pos()
assert max(mpos) < 200
delta = ((mpos - pt) / 200) * (3 * np.pi / 2)
