def draw_arm(self, arm, color='black'):
p1 = Point2().polar(arm.l1, arm.t1)
p2 = Point2().polar(arm.l2, arm.t2+arm.t1) + p1
self.ez.line(Point2(), p1, color=color)
self.ez.point(p1)
self.ez.line(p1, p2, color=color)
self.ez.point(p2)
def reset(self):
mse = self.mse()
print self.traj_step, mse
if self.loop == TEST_NETWORK:
self.net_logger.add_data(self.traj_step, mse)
elif self.loop == JACOBIAN:
self.jacobian_logger.add_data(self.traj_step, mse)
self.traj_step += TRAJ_STEP_INC
if self.traj_step > TRAJ_STEP_MAX:
self.exit()
self.trajectory = Trajectory(
step_length=self.traj_step,
restart=self.reset)
self.errors = []
self.arm = Arm()
if self.loop == JACOBIAN:
self.jacobian.arm = self.arm
# temporary
self.plot_errors.append(
Point2(self.traj_step * 10, mse*1000)
)
elif self.loop == TEST_NETWORK:
self.net.arm = self.arm
def spawnBall(self):
pos = Point2(random.randint(10, SCREEN_WIDTH - 10),
random.randint(10, SCREEN_HEIGHT - 10))
if (pos - DROPZONE_CENTER).r < DROPZONE_RADIUS:
self.spawnBall()
else:
self.balls.append(Ball(pos))
def draw_info(self):
# tamanho do passo atual
val = self.traj_step
val_text = "Step size.....{0}".format(val)
self.ez.text(val_text, Point2(10,10), onScreen=True)
# passo atual
val = self.trajectory.curr_idx()
val_text = "Current step..{0}".format(val)
self.ez.text(val_text, Point2(10,20), onScreen=True)
# erro medio
val = self.mse()
val_text = "Avg. error....{0}".format(val)
self.ez.text(val_text, Point2(10,30), onScreen=True)
if self.loop == JACOBIAN:
# temporary
if len(self.plot_errors) > 2:
self.ez.lines(self.plot_errors,color='blue')
def drawRobot(self):
pygame.draw.circle(screen, self.color, self.pos.rect(asInt=True),
ROBOT_RADIUS)
end_point = Point2()
end_point.r = ROBOT_RADIUS
end_point.a = self.orientation
end_point += self.pos
pygame.draw.line(screen, RED, self.pos.rect(asInt=True),
end_point.rect(asInt=True), 2)
def __init__(self,
color=STRANGE_BLUE,
x=SCREEN_WIDTH / 2,
y=SCREEN_HEIGHT / 2):
self.color = color
self.pos = Point2(x, y)
self.orientation = 0.0
self.picked_balls = []
def __init__(self, name, color, x=SCREEN_WIDTH / 2, y=SCREEN_HEIGHT / 2):
self.color = color
self.name = name
self.score = 0 # how many balls this robot dropped correctly
self.pos = Point2(x, y)
self.orientation = 0.0
self.picked_balls = []
self.picking = False
self.dropping = False
def spawnBall(self):
pos = Point2(random.randint(10, SCREEN_WIDTH - 10),
random.randint(10, SCREEN_HEIGHT - 10))
# creates a ball in front of the initial position of the robot
# for debugging
# pos = Point2(SCREEN_WIDTH/2 + DROPZONE_RADIUS*2, SCREEN_HEIGHT/2)
if (pos - DROPZONE_CENTER).r < DROPZONE_RADIUS + 30:
self.spawnBall()
else:
self.balls.append(Ball(pos))
def drawRobot(self):
pygame.draw.circle(screen, self.color, self.pos.rect(asInt=True),
ROBOT_RADIUS)
end_point = Point2()
end_point.r = ROBOT_RADIUS
end_point.a = self.orientation
end_point += self.pos
pygame.draw.line(screen, RED, self.pos.rect(asInt=True),
end_point.rect(asInt=True), 2)
name_surface = name_font.render(self.name, True, WHITE)
name_width = name_surface.get_width()
pos = (self.pos.rect(asInt=True)[0] - name_width / 2,
self.pos.rect(asInt=True)[1] - ROBOT_RADIUS - 15)
screen.blit(name_surface, pos)
def generate_samples_loop(self):
sample = self.generate_sample()
vector = Point2(sample[2], sample[3])
if vector.r > MIN_STEP and vector.r < MAX_STEP:
norm_sample = self.normalize_sample(sample)
self.dataset_writer.writerow(norm_sample)
self.sample_counter += 1
else:
self.invalid_counter += 1
if self.sample_counter % 10000 == 0:
print "{} samples generated in this loop.".format(self.sample_counter)
avg = self.sample_counter/float((self.sample_counter + self.invalid_counter))
print "{}% valid".format(avg*100.)
if self.sample_counter >= 2000000:
self.exit()
def __init__(self, width=100, height=100):
self.width = width
self.height = height
self.size = (self.width, self.height)
# self.origin = Point2(self.width/2, self.height/2)
self.origin = Point2(300, self.height - 150)
self.screen = None
self.backgroundColor = '#FFFFFF'
self.axisColor = '#D0D0D0'
self.textColor = '#666666'
self.loopFunction = None
self.keepRunning = True
self.clock = pygame.time.Clock()
self.fps = 60
self.keys = {K_ESCAPE: self.setQuit}
self.gui = True
def setSize(self, w, h):
self.width = w
self.height = h
self.size = (self.width, self.height)
self.origin = Point2(self.width / 2, self.height / 2)
dt1_y[i] += 1
break
# check dt2
if DT2_DISTRIBUTION:
for i, interval in enumerate(dt2_x):
y = scale(0., 1., np.radians(-MAX_DELTA),
np.radians(MAX_DELTA), float(row[5]))
if y < interval:
dt2_y[i] += 1
break
# check step
if STEP_DISTRIBUTION:
for i, interval in enumerate(step_x):
dx = scale(0., 1., -MAX_STEP, MAX_STEP, float(row[2]))
dy = scale(0., 1., -MAX_STEP, MAX_STEP, float(row[3]))
p = Point2(dx, dy)
y = p.r
if y < interval:
step_y[i] += 1
break
samples_checked += 1
if samples_checked % 10000 == 0:
print "Samples checked: {}/{}".format(samples_checked, row_count)
# if samples_checked > 50000:
# break
if T1_DISTRIBUTION:
print t1_y
plt.figure(figsize=figsize)
plt.plot(t1_x, t1_y)
plt.legend([r'$\theta 1$'])
def generate(self):
p = Point2(self.radius, 0)
angle_step = 2*np.arcsin(self.step_length/(2.*self.radius))
while len(self.points) < self.points_count:
self.points.append(Point2(self.center+p))
p.a += angle_step
SCREEN_WIDTH = 800
SCREEN_HEIGHT = 600
BALL_RADIUS = 5
ROBOT_RADIUS = 30
# colors
BACKGROUND = (30, 30, 30)
DROPZONE_COLOR = (244, 144, 30)
STRANGE_BLUE = (0, 72, 82)
RED = (255, 0, 0)
GREEN = (0, 255, 0)
# simulation parameters
MAX_CARRY_BALLS = 3 # amount of balls a robot can carry
MAX_BALLS = 100 # max spawned balls
DROPZONE_CENTER = Point2((SCREEN_WIDTH / 2, SCREEN_HEIGHT / 2))
DROPZONE_RADIUS = 100
MOVE_STEP = 2
TURN_STEP = 0.02 # increment in radians to robot orientation
#CRTuple = namedtuple("Robot", "controller robot")
pygame.init()
screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT))
clock = pygame.time.Clock()
class Ball(object):
def __init__(self, pos):
self.pos = pos
self.owner = None
def transform(self, p):
# transforms world coordinates to screen coordinates
return Point2(p.x + self.origin.x, self.origin.y - p.y)
from point2 import Point2
import numpy as np
TRAJ_CENTER = Point2(200, 300)
class Trajectory:
def __init__(self, step_length=0.1, radius=100, points_count=1000, center=TRAJ_CENTER, restart=None):
self.points = []
self.counter = 0
self.center = center
self.radius = radius
self.step_length = step_length
self.points_count = points_count
self.restart_function = restart
self.generate()
def generate(self):
p = Point2(self.radius, 0)
angle_step = 2*np.arcsin(self.step_length/(2.*self.radius))
while len(self.points) < self.points_count:
self.points.append(Point2(self.center+p))
p.a += angle_step
def current(self):
return self.points[self.counter]
def curr_idx(self):
return self.counter
def next(self):
def move(self):
goal_position = Point2()
goal_position.r = MOVE_STEP
goal_position.a = self.orientation
self.pos += goal_position
def endeffector(self):
p1 = Point2().polar(self.l1, self.t1)
return Point2().polar(self.l2, self.t2+self.t1) + p1