def get_action(self, timestep_i, current_orientation, actions_checked=[]):
perlin_noise = noise.pnoise1(
(float(timestep_i * self.action_repeat) + self.offset0) /
self.scale0)
perlin_noise += noise.pnoise1(
(float(timestep_i * self.action_repeat) + self.offset1) /
self.scale1)
action = int(perlin_noise * self.max_scaler)
if action > self.max_scaler:
action = self.max_scaler
elif action < -self.max_scaler:
action = -self.max_scaler
action_samples = 0
while action in actions_checked and action_samples < 50:
action_samples += 1
self.reset_action()
perlin_noise = noise.pnoise1(
(float(timestep_i * self.action_repeat) + self.offset0) /
self.scale0)
perlin_noise += noise.pnoise1(
(float(timestep_i * self.action_repeat) + self.offset1) /
self.scale1)
action = int(perlin_noise * self.max_scaler)
if action > self.max_scaler:
action = self.max_scaler
elif action < -self.max_scaler:
action = -self.max_scaler
steering_force = vector(action * self.wander_range +
current_orientation)
return action, steering_force
def turn_robot_around(self):
previous_angle = vector(self.robot.body.angle)
previous_position = self.robot.body.position
steering_vector = -previous_angle + .01 * np.random.randn(2)
turn_len = random.randint(0, 1)
if turn_len == 0:
for i in range(180):
self.step(steering_vector, ignore_collisions=True)
else:
for i in range(250):
self.step(steering_vector, ignore_collisions=True)
self.robot.body.angular_velocity = 0
self.robot.body.velocity = (0, 0)
def get_action(self, current_position, current_orientation):
seek_vector = self.target_position - current_position
# if la.norm(seek_vector) < 50:
# print('GOAL')
# pdb.set_trace()
# print(la.norm(seek_vector))
steering_vector = seek_vector - vector(current_orientation)
action_space = np.arange(-5, 6)
min_diff = 9999999
min_a = 0
for a in action_space:
steering_force = vector(a * self.wander_range +
current_orientation)
diff = la.norm(steering_force - steering_vector)
if diff <= min_diff:
min_a = a
min_diff = diff
steering_force = vector(min_a * self.wander_range +
current_orientation)
return min_a, steering_force
def _reset_robot(self, center=False, collision_points=None):
previous_angle = vector(self.robot.body.angle)
previous_position = self.robot.body.position
if collision_points:
self.robot.body.angle = angle(collision_points[0].point_a - collision_points[0].point_b)
else:
self.robot.body.angle = angle(-previous_angle)
if center:
self.robot.body.position = self.CENTER
else:
self.robot.body.position = previous_position - (previous_angle * 25)
self.robot.body.angular_velocity = 0
self.robot.body.velocity = (0,0)
def add_sensors(self, sensor_range=150.0):
self.sensor_range = sensor_range
sensor_shapes = []
sensor_end_points = []
sensor_angles = [66, 33, 0, -33, -66]
for a in sensor_angles:
angle = self.body.angle + math.radians(a)
v = vector(angle)
p = v * sensor_range
sensor_end_points.append(p)
thickness = 1
for p in sensor_end_points:
sensor_shape = pm.Segment(self.body, (0, 0), p, thickness)
sensor_shape.color = BLUE
sensor_shape.sensor = True
sensor_shapes.append(sensor_shape)
return sensor_shapes, sensor_angles, sensor_range
def raycasting(self, print_sensors=False):
robot_filter = pm.ShapeFilter(mask=pm.ShapeFilter.ALL_MASKS ^ 0b1)
sensor_end_points=[]
for a in self.robot.sensor_angles:
angle = self.robot.body.angle + math.radians(a)
v = vector(angle)
p = v * self.robot.sensor_range + self.robot.body.position
sensor_end_points.append(p)
segment_queries = []
for i, p in enumerate(sensor_end_points):
segment_query = self.space.segment_query_first(self.robot.body.position,p,1,robot_filter)
if segment_query:
segment_queries.append(la.norm(segment_query.point - self.robot.body.position))
else:
segment_queries.append(self.robot.sensor_range)
sq = np.array(segment_queries)
if print_sensors:
print("%d %d %d %d %d"%(int(sq[0]),int(sq[1]),int(sq[2]),int(sq[3]),int(sq[4])))
return sq
def get_steering_force(self, action, current_orientation):
steering_force = vector(action * self.wander_range +
current_orientation)
return steering_force
