class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.odometry = Odometry()
self.pd_controller = PDController(500, 100, -75, 75)
# self.pid_controller = PIDController(500, 100, 0, -75, 75, -50, 50)
self.pid_controller = PIDController(500, 100, 10, -100, 100, -100, 100)
self.pid_controller_dist = PIDController(500, 100, 10, -100, 100, -100,
100)
def sleep(self, time_in_sec):
"""Sleeps for the specified amount of time while keeping odometry up-to-date
Args:
time_in_sec (float): time to sleep in seconds
"""
start = self.time.time()
while True:
state = self.create.update()
if state is not None:
self.odometry.update(state.leftEncoderCounts,
state.rightEncoderCounts)
# print("[{},{},{}]".format(self.odometry.x, self.odometry.y, np.rad2deg(self.odometry.theta)))
t = self.time.time()
if start + time_in_sec <= t:
break
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
plt_time_arr = np.array([])
plt_angle_arr = np.array([])
plt_goal_angle_arr = np.array([])
goal_x = -0.5
goal_y = -0.5
goal_coor = np.array([goal_x, goal_y])
goal_angle = np.arctan2(goal_y, goal_x)
goal_angle %= 2 * np.pi
print("goal angle = %f" % np.rad2deg(goal_angle))
base_speed = 100
# timeout = abs(17 * (goal_angle / np.pi)) + 1
goal_r = 0.05
angle = self.odometry.theta
dist_to_goal = dist(goal_coor,
np.array([self.odometry.x, self.odometry.y]))
plt_time_arr = np.append(plt_time_arr, self.time.time())
plt_angle_arr = np.append(plt_angle_arr, angle)
plt_goal_angle_arr = np.append(plt_goal_angle_arr, goal_angle)
while abs(dist_to_goal) > goal_r:
goal_angle = np.arctan2(goal_y - self.odometry.y,
goal_x - self.odometry.x)
goal_angle %= 2 * np.pi
print("(%f, %f), dist to goal = %f\n" %
(self.odometry.x, self.odometry.y, dist_to_goal))
dist_to_goal = dist(goal_coor,
np.array([self.odometry.x, self.odometry.y]))
angle = self.odometry.theta
plt_angle_arr = np.append(plt_angle_arr, angle)
plt_goal_angle_arr = np.append(plt_goal_angle_arr, goal_angle)
# output = self.pd_controller.update(angle, goal_angle, self.time.time())
output = self.pid_controller.update(angle, goal_angle,
self.time.time())
output_dist = self.pid_controller_dist.update(
0, dist_to_goal, self.time.time())
self.create.drive_direct(int(base_speed + output),
int(base_speed - output))
# self.create.drive_direct(
# int((dist_to_goal * base_speed) + output),
# int((dist_to_goal * base_speed) - output)
# )
# self.create.drive_direct(
# int(output_dist + output),
# int(output_dist - output)
# )
self.sleep(0.01)
np.savetxt("timeOutput.csv", plt_time_arr, delimiter=",")
np.savetxt("angleOutput.csv", plt_angle_arr, delimiter=",")
np.savetxt("goalAngleOutput.csv", plt_goal_angle_arr, delimiter=",")
class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
# read file and get all paths
self.img = lab11_image.VectorImage("lab11_img1.yaml")
# init objects
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.penholder = factory.create_pen_holder()
self.tracker = Tracker(factory.create_tracker,
1,
sd_x=0.01,
sd_y=0.01,
sd_theta=0.01,
rate=10)
self.servo = factory.create_servo()
self.odometry = Odometry()
# alpha for tracker
self.alpha_x = 0.3
self.alpha_y = self.alpha_x
self.alpha_theta = 0.3
# init controllers
self.pidTheta = PIDController(200,
5,
50, [-10, 10], [-200, 200],
is_angle=True)
self.pidDistance = PIDController(500,
0,
50, [0, 0], [-200, 200],
is_angle=False)
self.filter = ComplementaryFilter(
self.odometry, None, self.time, 0.4,
(self.alpha_x, self.alpha_y, self.alpha_theta))
# parameters
self.base_speed = 100
# constant
self.robot_marker_distance = 0.1906
# debug vars
self.debug_mode = True
self.odo = []
self.actual = []
self.xi = 0
self.yi = 0
self.init = True
self.penholder_depth = -0.025
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
self.penholder.set_color(0.0, 1.0, 0.0)
# generate spline points and line drawing order
splines, splines_color = PathFinder.get_spline_points(self.img.paths)
# in format [index, is_parallel, is_spline]
line_index_list = PathFinder.find_path(self.img.lines, splines,
splines_color)
prev_color = None
# loop to draw all lines and paths
for draw_info in line_index_list:
index = int(draw_info[0])
is_parallel = draw_info[1]
is_spline = draw_info[2]
line = self.img.lines[index]
curr_color = self.img.lines[index].color
if curr_color != prev_color:
prev_color = curr_color
self.penholder.set_color(*get_color(curr_color))
# ===== spline routine =====
if is_spline:
path_points = self.draw_path_coords(splines[index],
is_parallel)
self.penholder.set_color(*get_color(splines_color[0]))
# go to start of the curve and begin drawing
for i in range(0, 2):
# go to start of curve
goal_x, goal_y = path_points[0, 0], path_points[0, 1]
print("=== GOAL SET === {:.3f}, {:.3f}".format(
goal_x, goal_y))
if i == 1:
goal_x, goal_y = path_points[9, 0], path_points[9, 1]
# turn to goal
# self.tracker.update()
self.filter.update()
curr_x = self.filter.x
curr_y = self.filter.y
goal_theta = math.atan2(goal_y - curr_y, goal_x - curr_x)
if i == 1:
goal_theta = math.atan2(goal_y - path_points[0, 1],
goal_x - path_points[0, 0])
self.penholder.go_to(0.0)
self.go_to_angle(goal_theta)
self.go_to_goal(goal_x, goal_y)
# start drawing after correctly oriented
# uses only odometry during spline drawing
self.penholder.go_to(self.penholder_depth)
prev_base_speed = self.base_speed
self.filter.updateFlag = False
self.base_speed = 25
print("Draw!")
last_drew_index = 0
# draw the rest of the curve. Draws every 10th point.
for i in range(10, len(path_points), 5):
goal_x, goal_y = path_points[i, 0], path_points[i, 1]
print("=== GOAL SET === {:.3f}, {:.3f}".format(
goal_x, goal_y))
self.go_to_goal(goal_x, goal_y, useOdo=True)
print("\nlast drew index {}\n".format(last_drew_index))
if last_drew_index < len(path_points) - 1:
goal_x, goal_y = path_points[-1, 0], path_points[-1, 1]
print("=== GOAL SET === {:.3f}, {:.3f}".format(
goal_x, goal_y))
self.go_to_goal(goal_x, goal_y, useOdo=False)
# stop drawing and restore parameter values
self.base_speed = prev_base_speed
self.filter.updateFlag = True
self.penholder.go_to(0.0)
# ===== line routine =====
else:
for i in range(0, 2):
goal_x, goal_y = self.draw_coords(line,
is_parallel=is_parallel,
at_start=True)
if i == 1:
goal_x, goal_y = self.draw_coords(
line, is_parallel=is_parallel, at_start=False)
print("=== GOAL SET === {:.3f}, {:.3f}".format(
goal_x, goal_y))
# turn to goal
# self.tracker.update()
self.filter.update()
curr_x = self.filter.x
curr_y = self.filter.y
goal_theta = math.atan2(goal_y - curr_y, goal_x - curr_x)
self.penholder.go_to(0.0)
self.go_to_angle(goal_theta)
if i == 1:
# start drawing
self.penholder.go_to(self.penholder_depth)
print("Draw!")
self.go_to_goal(goal_x, goal_y)
# graph the final result
self.draw_graph()
self.create.stop()
def drive(self, theta, distance, speed):
# Sum all controllers and clamp
self.create.drive_direct(
max(min(int(theta + distance + speed), 500), -500),
max(min(int(-theta + distance + speed), 500), -500))
def sleep(self, time_in_sec):
"""Sleeps for the specified amount of time while keeping odometry up-to-date
Args:
time_in_sec (float): time to sleep in seconds
"""
start = self.time.time()
while True:
self.update()
t = self.time.time()
if start + time_in_sec <= t:
break
# gives coordinates to draw the lines correctly
# line: segment to be drawn
# at_start: set true to retun the first coordinate, set false for the second coordinate
# returns the x, y coordinates offset
def draw_coords(self, line, at_start=True, is_parallel=True):
if is_parallel:
theta = math.atan2(line.v[1] - line.u[1],
line.v[0] - line.u[0]) + math.pi / 2
if at_start:
return math.cos(theta) * self.robot_marker_distance + line.u[0], \
math.sin(theta) * self.robot_marker_distance + line.u[1]
else:
return math.cos(theta) * self.robot_marker_distance + line.v[0], \
math.sin(theta) * self.robot_marker_distance + line.v[1]
else:
theta = math.atan2(line.v[1] - line.u[1],
line.v[0] - line.u[0]) - math.pi / 2
if at_start:
return math.cos(theta) * self.robot_marker_distance + line.v[0], \
math.sin(theta) * self.robot_marker_distance + line.v[1]
else:
return math.cos(theta) * self.robot_marker_distance + line.u[0], \
math.sin(theta) * self.robot_marker_distance + line.u[1]
def draw_path_coords(self, result, is_parallel):
final_result = np.empty((0, 2))
if is_parallel:
for i in range(0, len(result) - 1):
theta = math.atan2(
result[i, 1] - result[i + 1, 1],
result[i, 0] - result[i + 1, 0]) - math.pi / 2
s = math.cos(theta) * self.robot_marker_distance + result[i, 0], \
math.sin(theta) * self.robot_marker_distance + result[i, 1]
final_result = np.vstack([final_result, s])
else:
for i in range(len(result) - 1, 1, -1):
theta = math.atan2(
result[i, 1] - result[i - 1, 1],
result[i, 0] - result[i - 1, 0]) - math.pi / 2
s = math.cos(theta) * self.robot_marker_distance + result[i, 0], \
math.sin(theta) * self.robot_marker_distance + result[i, 1]
final_result = np.vstack([final_result, s])
return final_result
def go_to_goal(self, goal_x, goal_y, useOdo=False):
while True:
state = self.update()
if state is not None:
if useOdo:
curr_x = self.odometry.x
curr_y = self.odometry.y
curr_theta = self.odometry.theta
else:
curr_x = self.filter.x
curr_y = self.filter.y
curr_theta = self.filter.theta
distance = math.sqrt(
math.pow(goal_x - curr_x, 2) +
math.pow(goal_y - curr_y, 2))
output_distance = self.pidDistance.update(
0, distance, self.time.time())
theta = math.atan2(goal_y - curr_y, goal_x - curr_x)
output_theta = self.pidTheta.update(curr_theta, theta,
self.time.time())
print("goal x,y = {:.3f}, {:.3f}, x,y = {:.3f}, {:.3f}".format(
goal_x, goal_y, curr_x, curr_y))
self.drive(output_theta, output_distance, self.base_speed)
if distance < 0.05:
self.create.drive_direct(0, 0)
break
self.sleep(0.01)
def go_to_angle(self, goal_theta):
curr_theta = self.filter.theta
while abs(-math.degrees(
math.atan2(math.sin(curr_theta - goal_theta),
math.cos(curr_theta - goal_theta)))) > 8:
curr_theta = self.filter.theta
print("goal_theta = {:.2f}, theta = {:.2f}".format(
math.degrees(goal_theta), math.degrees(curr_theta)))
output_theta = self.pidTheta.update(curr_theta, goal_theta,
self.time.time())
self.drive(output_theta, 0, 0)
self.sleep(0.01)
self.create.drive_direct(0, 0)
# debug function. Draws robot paths
def draw_graph(self):
# show drawing progress after each line segment is drawn
if self.debug_mode:
if len(self.odo) is not 0 and len(self.actual) is not 0:
x, y = zip(*self.odo)
a, b = zip(*self.actual)
plt.plot(x, y, color='red', label='Sensor path')
plt.plot(a,
b,
color='green',
label='Actual path',
linewidth=1.4)
self.odo = []
self.actual = []
for path in self.img.paths:
ts = np.linspace(0, 1.0, 100)
result = np.empty((0, 3))
for i in range(0, path.num_segments()):
for t in ts[:-2]:
s = path.eval(i, t)
result = np.vstack([result, s])
plt.plot(result[:, 0], result[:, 1], path.color)
path_points = self.draw_path_coords(result, True)
plt.plot(path_points[:, 0], path_points[:, 1], color='aqua')
path_points = self.draw_path_coords(result, False)
plt.plot(path_points[:, 0], path_points[:, 1], color='aqua')
for line in self.img.lines:
# draw lines
plt.plot([line.u[0], line.v[0]], [line.u[1], line.v[1]],
line.color)
theta = math.atan2(line.v[1] - line.u[1],
line.v[0] - line.u[0]) + math.pi / 2
plt.plot([
math.cos(theta) * self.robot_marker_distance + line.u[0],
math.cos(theta) * self.robot_marker_distance + line.v[0]
], [
math.sin(theta) * self.robot_marker_distance + line.u[1],
math.sin(theta) * self.robot_marker_distance + line.v[1]
], 'aqua')
theta = math.atan2(line.v[1] - line.u[1],
line.v[0] - line.u[0]) - math.pi / 2
plt.plot([
math.cos(theta) * self.robot_marker_distance + line.u[0],
math.cos(theta) * self.robot_marker_distance + line.v[0]
], [
math.sin(theta) * self.robot_marker_distance + line.u[1],
math.sin(theta) * self.robot_marker_distance + line.v[1]
], 'aqua')
plt.legend()
plt.show()
# updates odometry, filter, and tracker
def update(self):
state = self.create.update()
self.filter.update()
# self.tracker.update()
if state is not None:
self.odometry.update(state.leftEncoderCounts,
state.rightEncoderCounts)
# print("[{},{},{}]".format(self.odometry.x, self.odometry.y, math.degrees(self.odometry.theta)))
if self.debug_mode:
self.odo.append((self.filter.x, self.filter.y))
self.actual.append(
(self.create.sim_get_position()[0] - self.xi,
self.create.sim_get_position()[1] - self.yi))
return state
class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
self.map = np.loadtxt("map14.txt", delimiter=",")
self.odometry = Odometry()
self.search = AStar(self.map)
self.location = (0,0)
self.orientation = "east"
self.pidTheta = pid_controller.PIDController(200, 25, 5, [-1, 1], [-200, 200], is_angle=True)
self.pidDistance = pid_controller.PIDController(100, 15, 5, [-10, 10], [-200, 200], is_angle=False)
self.base_speed = 50
def goToGoal(self, goal, state):
goal_y = goal[1] / 30
goal_x = goal[0] / 30
self.odometry.update(state.leftEncoderCounts, state.rightEncoderCounts)
goal_theta = math.atan2(goal_y - self.odometry.y, goal_x - self.odometry.x)
theta = math.atan2(math.sin(self.odometry.x), math.cos(self.odometry.y))
output_theta = self.pidTheta.update(self.odometry.theta, goal_theta, self.time.time())
# improved version 2: fuse with velocity controller
self.distance = math.sqrt(math.pow(goal_x - self.odometry.x, 2) + math.pow(goal_y - self.odometry.y, 2))
output_distance = self.pidDistance.update(0, self.distance, self.time.time())
self.create.drive_direct(int(output_theta + output_distance)+self.base_speed, int(-output_theta + output_distance)+self.base_speed)
return output_distance
def shouldIContinueAlongMyPath(self):
#First scan ahead
distance = self.sonar.get_distance()
if distance < 0.5:
#Then add the obstacle to the map
if self.orientation == "east":
self.map[(self.location[0], self.location[1]+1)] = 1
elif self.orientation == "west":
self.map[(self.location[0], self.location[1]-1)] = 1
elif self.orientation == "north":
self.map[(self.location[0]-1, self.location[1])] = 1
else:
self.map[(self.location[0]+1, self.location[1])] = 1
return False
return True
def moveOneSquare(self, goal):
startX = self.odometry.x
startY = self.odometry.y
while True:
print("Moving")
#Get state and update the odometry
self.state = self.create.update()
while not self.state:
self.state = self.create.update()
if abs(self.goToGoal(goal, self.state)) < 10:
break
if self.orientation == "east":
self.location = (self.location[0], self.location[1]+1)
elif self.orientation == "west":
self.location = (self.location[0], self.location[1]-1)
elif self.orientation == "south":
self.location = (self.location[0]+1, self.location[1])
else:
self.location = (self.location[0]-1, self.location[1]+1)
def turnCreate(self, goalTheta, state, pos):
self.odometry.update(state.leftEncoderCounts, state.rightEncoderCounts)
theta = math.atan2(math.sin(self.odometry.theta), math.cos(self.odometry.theta))
output_theta = self.pidTheta.update(self.odometry.theta, goalTheta, self.time.time())
# improved version 2: fuse with velocity controller
if pos:
self.create.drive_direct(int(output_theta), int(-output_theta))
else:
self.create.drive_direct(-int(output_theta), int(output_theta))
return output_theta
def turnTowardsPoint(self, nextPoint):
startTheta = self.odometry.theta
if nextPoint[0] < self.location[0]:
print("Turning north")
#Then turn north
goalTheta = math.pi / 2
while True:
#Get state
self.state = self.create.update()
while not self.state:
self.state = self.create.update()
if abs(self.turnCreate(goalTheta, self.state, True)) < 10:
break
print("Done turning")
self.orientation = "north"
elif nextPoint[0] > self.location[0]:
print("Turning south")
#Then turn south
goalTheta = 3 * math.pi / 2
while True:
#Get state
self.state = self.create.update()
while not self.state:
self.state = self.create.update()
if abs(self.turnCreate(goalTheta, self.state, True)) < 10:
break
print("Done turning")
self.orientation = "south"
elif nextPoint[1] < self.location[1]:
print("Turning west")
#Then turn west
goalTheta = math.pi
while True:
#Get state
self.state = self.create.update()
while not self.state:
self.state = self.create.update()
if abs(self.turnCreate(goalTheta, self.state, True)) < 10:
break
print("Done turning")
self.orientation = "west"
else:
print("Turning east")
#Then turn east
goalTheta = 0
while True:
#Get state
self.state = self.create.update()
while not self.state:
self.state = self.create.update()
if abs(self.turnCreate(goalTheta, self.state, True)) < 10:
break
print("Done turning")
self.orientation = "east"
def goOnPath(self, path):
for x, y in path:
if (x, y) == self.location:
pass
#Turn towards the point
print("Turning towards Point ", (x, y))
self.turnTowardsPoint((x,y))
#If point is now obstacle, go in a new path
if self.shouldIContinueAlongMyPath():
#Move towards point
print("No obstacle. Moving toward point")
self.moveOneSquare((x,y))
else:
return False
return True
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
self.state = self.create.update()
while not self.state:
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
self.location = (9,1)#(self.odometry.x, self.odometry.y)
goal_pos = (1,9)
path = self.search.a_star(self.location, goal_pos)
print("Path is: ",path)
#Now go on that path
while not self.goOnPath(path):
path = self.search.a_star(self.location, goal_pos)
path.remove(self.location)
print("New Path is: ", path)
print("Done! I am at goal.")
class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.map = lab8_map.Map("lab8_map.json")
self.odometry = Odometry()
self.odometry.x = 1
self.odometry.y = 0.5
self.pidTheta = PIDController(300,
5,
50, [-10, 10], [-200, 200],
is_angle=True)
# constants
self.base_speed = 100
self.variance_sensor = 0.1
self.variance_distance = 0.01
self.variance_direction = 0.05
self.world_width = 3.0 # the x
self.world_height = 3.0 # the y
self.travel_dist = 0.25
self.min_dist_to_wall = self.travel_dist + 0.2
self.min_dist_to_localize = 0.2
self.min_theta_to_localize = math.pi / 4
self.n_threshold = math.log(0.09)
self.filter = ParticleFilter(self.virtual_create,
self.variance_sensor,
self.variance_distance,
self.variance_direction,
num_particles=100,
world_width=self.world_width,
world_height=self.world_height,
input_map=self.map,
n_threshold=self.n_threshold)
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
isLocalized = False
angle_counter = 0
# This is an example on how to detect that a button was pressed in V-REP
while not isLocalized:
self.draw_particles()
# generate random num
command = random.choice([c for c in Command])
if command is Command.FORWARD:
if self.sonar.get_distance() < self.min_dist_to_wall:
continue
self.filter.move(0, self.travel_dist)
# 100 mm/s = 0.1 m/s
self.create.drive_direct(self.base_speed, self.base_speed)
self.sleep(abs(self.travel_dist / 0.1))
# stop
self.create.drive_direct(0, 0)
elif command is Command.TURN_LEFT:
# turn_angle = math.pi / 2 + self.odometry.theta
turn_angle = math.pi / 2 + angle_counter
turn_angle %= 2 * math.pi
angle_counter += math.pi / 2
angle_counter %= 2 * math.pi
self.filter.move(turn_angle, 0)
# turn left by 90 degree
self.go_to_angle(turn_angle)
elif command is Command.TURN_RIGHT:
# turn_angle = -math.pi / 2 + self.odometry.theta
turn_angle = -math.pi / 2 + angle_counter
turn_angle %= 2 * math.pi
angle_counter -= math.pi / 2
angle_counter %= 2 * math.pi
self.filter.move(turn_angle, 0)
# turn right by 90 degree
self.go_to_angle(turn_angle)
self.filter.sense(self.sonar.get_distance())
# check if localized
# distance between odometry and estimated positions
dist_position_to_goal = math.sqrt(
(self.odometry.x - self.filter.x)**2 +
(self.odometry.y - self.filter.y)**2)
diff_theta_to_goal = abs(self.odometry.theta - self.filter.theta)
isLocalized = dist_position_to_goal < self.min_dist_to_localize and diff_theta_to_goal < self.min_theta_to_localize
def go_to_angle(self, goal_theta):
while abs(
math.atan2(math.sin(goal_theta - self.odometry.theta),
math.cos(goal_theta - self.odometry.theta))) > 0.1:
# print("Go TO: " + str(math.degrees(goal_theta)) + " " + str(math.degrees(self.odometry.theta)))
output_theta = self.pidTheta.update(self.odometry.theta,
goal_theta, self.time.time())
self.create.drive_direct(int(+output_theta), int(-output_theta))
self.sleep(0.01)
self.create.drive_direct(0, 0)
def sleep(self, time_in_sec):
"""Sleeps for the specified amount of time while keeping odometry up-to-date
Args:
time_in_sec (float): time to sleep in seconds
"""
start = self.time.time()
while True:
state = self.create.update()
if state is not None:
self.odometry.update(state.leftEncoderCounts,
state.rightEncoderCounts)
# print("[{},{},{}]".format(self.odometry.x, self.odometry.y, math.degrees(self.odometry.theta)))
t = self.time.time()
if start + time_in_sec <= t:
break
def draw_particles(self):
data = []
# get position data from all particles
for particle in self.filter.particles:
data.extend([particle.x, particle.y, 0.1, particle.theta])
# paint all particles in simulation
self.virtual_create.set_point_cloud(data)
class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.odometry = Odometry()
self.x_arr = np.array([])
self.y_arr = np.array([])
self.x_arr_truth = np.array([])
self.y_arr_truth = np.array([])
self.time_arr = np.array([])
def sleep(self, time_in_sec):
"""Sleeps for the specified amount of time while keeping odometry up-to-date
Args:
time_in_sec (float): time to sleep in seconds
"""
start = self.time.time()
t = start
while t - start < time_in_sec:
state = self.create.update()
if state is not None:
self.odometry.update(state.leftEncoderCounts, state.rightEncoderCounts)
# print("[{},{},{}]".format(self.odometry.x, self.odometry.y, np.rad2deg(self.odometry.theta)))
self.time_arr = np.append(self.time_arr, t)
self.x_arr = np.append(self.x_arr, self.odometry.x)
self.y_arr = np.append(self.y_arr, self.odometry.y)
self.x_arr_truth = np.append(self.x_arr_truth, self.create.sim_get_position()[0])
self.y_arr_truth = np.append(self.y_arr_truth, self.create.sim_get_position()[1])
t = self.time.time()
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
speed = 300 # mm/s
time_90deg_turn = 1.9 / (speed / 100.0) # seconds
# move forward for 1m
self.create.drive_direct(speed, speed)
self.sleep(1000 / speed)
# turn left 90 deg
self.create.drive_direct(speed, -speed)
self.sleep(time_90deg_turn)
# move forward for 0.5m
self.create.drive_direct(speed, speed)
self.sleep(500 / speed)
# turn left 90 deg
self.create.drive_direct(speed, -speed)
self.sleep(time_90deg_turn)
# move forward for 1m
self.create.drive_direct(speed, speed)
self.sleep(1000 / speed)
# turn left 90 deg
self.create.drive_direct(speed, -speed)
self.sleep(time_90deg_turn)
# move forward for 0.5m
self.create.drive_direct(speed, speed)
self.sleep(500 / speed)
# stop simulation
self.create.stop()
x_offset = self.x_arr_truth[np.argwhere(self.time_arr > 0.05)[0]]
y_offset = self.y_arr_truth[np.argwhere(self.time_arr > 0.05)[0]]
# plt.plot(self.time_arr, self.x_arr + x_offset, label='x coor measured')
# plt.plot(self.time_arr, self.x_arr_truth, '--', label='x coor truth')
# plt.plot(self.time_arr, self.y_arr + y_offset, label='y coor measured')
# plt.plot(self.time_arr, self.y_arr_truth, '--', label='y coor truth')
plt.plot(self.x_arr + x_offset, self.y_arr + y_offset, label='coor measured')
plt.plot(self.x_arr_truth, self.y_arr_truth, '--', label='coor truth')
plt.legend()
plt.grid()
plt.show()
class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.map = lab10_map.Map("lab11.map")
self.odometry = Odometry()
def showParticles(self, particles):
self.virtual_create.set_point_cloud(particles)
def visualizePose(self,x,y,z,theta):
self.virtual_create.set_pose((x, y, z), theta)
def turnLeftOneSec(self):
self.create.drive_direct(50,-50)
self.time.sleep(1)
self.create.drive_direct(0,0)
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
self.pf.TurnLeft()
def redrawParticles(self):
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
def findAndGoOnClearPath(self):
#Find new angle to move at
foundNewAngle = False
foundStartTime = False
runTime = 0
#Turn robot until clear space is found
while not foundNewAngle:
#Turn robot
self.turnLeftOneSec()
self.redrawParticles()
distanceFromNearestObstacle = self.sonar.get_distance()
obstacleAhead = distanceFromNearestObstacle < 0.001
if not obstacleAhead:
if not foundStartTime:
foundStartTime = True
else:
runTime += 1
if foundStartTime and runTime >= 4:
foundNewAngle = True
elif obstacleAhead and runTime < 4:
foundStartTime = False
runTime = 0
#Now turn the other way
turnRightTime = int(runTime / 2)
for i in range(0, turnRightTime):
self.create.drive_direct(-50,50)
self.time.sleep(1)
self.create.drive_direct(0,0)
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
self.pf.TurnRight()
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
#Actually move forward
self.create.drive_direct(50,50)
self.time.sleep(1)
self.create.drive_direct(0,0)
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
self.pf.MoveForward()
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
# This is an example on how to visualize the pose of our estimated position
# where our estimate is that the robot is at (x,y,z)=(0.5,0.5,0.1) with heading pi
#self.visualizePose(0.5, 0.5, 0.1, math.pi)
# This is an example on how to show particles
# the format is x,y,z,theta,x,y,z,theta,...
#data = [0.5, 0.5, 0.1, math.pi/2, 1.5, 1, 0.1, 0]
#self.showParticles(data)
# This is an example on how to estimate the distance to a wall for the given
# map, assuming the robot is at (0, 0) and has heading math.pi
#print(self.map.closest_distance((0.5,0.5), math.pi))
self.state = self.create.update()
while not self.state:
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
#This is an example on how to use PF
variances = [0.01,0.01,0.3]
self.pf = PF([self.map.bottom_left[0], self.map.top_right[0]],[self.map.bottom_left[1], self.map.top_right[1]],[0,2*math.pi], variances, self.map)
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
# This is an example on how to detect that a button was pressed in V-REP
bNum = 0
while True:
self.create.drive_direct(0,0)
self.state = self.create.update()
if self.state:
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
prevX = self.odometry.x
prevY = self.odometry.y
prevTheta = self.odometry.theta
#b = self.virtual_create.get_last_button()
if self.pf.isOneCluster():
break
if bNum % 2 == 0:
b = self.virtual_create.Button.Sense
elif bNum % 4 == 1:
b = self.virtual_create.Button.TurnLeft
elif bNum % 4 == 3:
b = self.virtual_create.Button.MoveForward
bNum += 1
if b == self.virtual_create.Button.MoveForward:
print("Forward pressed!")
self.findAndGoOnClearPath()
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
elif b == self.virtual_create.Button.TurnLeft:
print("Turn Left pressed!")
self.create.drive_direct(50,-50)
self.time.sleep(1)
self.create.drive_direct(0,0)
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
self.pf.TurnLeft()
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
elif b == self.virtual_create.Button.TurnRight:
print("Turn Right pressed!")
self.create.drive_direct(-50,50)
self.time.sleep(1)
self.create.drive_direct(0,0)
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
self.pf.TurnRight()
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
elif b == self.virtual_create.Button.Sense:
print("Sense pressed!")
dist = self.sonar.get_distance()
if dist:
self.pf.Sensing(dist)
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
self.time.sleep(0.01)
class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.odometry = Odometry()
self.pd_controller = PDController(500, 100, -75, 75)
# self.pid_controller = PIDController(500, 100, 0, -75, 75, -50, 50)
self.pid_controller = PIDController(250, 30, 1, -100, 100, -100, 100)
def sleep(self, time_in_sec):
"""Sleeps for the specified amount of time while keeping odometry up-to-date
Args:
time_in_sec (float): time to sleep in seconds
"""
start = self.time.time()
while True:
state = self.create.update()
if state is not None:
self.odometry.update(state.leftEncoderCounts,
state.rightEncoderCounts)
# print("[{},{},{}]".format(self.odometry.x, self.odometry.y, np.rad2deg(self.odometry.theta)))
t = self.time.time()
if start + time_in_sec <= t:
break
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
plt_time_arr = np.array([])
plt_angle_arr = np.array([])
goal_angle = np.pi / 2
goal_angle %= 2 * np.pi
base_speed = 0
timeout = abs(17 * (goal_angle / np.pi)) + 1
angle = self.odometry.theta
plt_time_arr = np.append(plt_time_arr, self.time.time())
plt_angle_arr = np.append(plt_angle_arr, angle)
start_time = self.time.time()
while self.time.time() - start_time < timeout:
angle = self.odometry.theta
plt_time_arr = np.append(plt_time_arr, self.time.time())
plt_angle_arr = np.append(plt_angle_arr, angle)
# output = self.pd_controller.update(angle, goal_angle, self.time.time())
output = self.pid_controller.update(angle, goal_angle,
self.time.time())
# print("angle =%f, output = %f" % (np.rad2deg(angle), output))
# print("[r = %f, l = %f]\n" % (int(base_speed + output), int(base_speed - output)))
self.create.drive_direct(int(base_speed + output),
int(base_speed - output))
self.sleep(0.01)
np.savetxt("timeOutput.csv", plt_time_arr, delimiter=",")
np.savetxt("angleOutput.csv", plt_angle_arr, delimiter=",")
class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.map = lab8_map.Map("lab8_map.json")
self.odometry = Odometry()
self.pidTheta = PIDController(300,
5,
50, [-10, 10], [-200, 200],
is_angle=True)
# constants
self.base_speed = 100
self.variance_sensor = 0.1
self.variance_distance = 0.01
self.variance_direction = 0.05
self.world_width = 3.0 # the x
self.world_height = 3.0 # the y
self.filter = ParticleFilter(self.virtual_create,
self.variance_sensor,
self.variance_distance,
self.variance_direction,
num_particles=100,
world_width=self.world_width,
world_height=self.world_height)
def run(self):
# # This is an example on how to visualize the pose of our estimated position
# # where our estimate is that the robot is at (x,y,z)=(0.5,0.5,0.1) with heading pi
# self.virtual_create.set_pose((0.5, 0.5, 0.1), 0)
#
# # This is an example on how to show particles
# # the format is x,y,z,theta,x,y,z,theta,...
# data = [0.5, 0.5, 0.1, math.pi/2, 1.5, 1, 0.1, 0]
# self.virtual_create.set_point_cloud(data)
#
# # This is an example on how to estimate the distance to a wall for the given
# # map, assuming the robot is at (0, 0) and has heading math.pi
# print(self.map.closest_distance((0.5,0.5), 0))
#
# # This is an example on how to detect that a button was pressed in V-REP
# while True:
# b = self.virtual_create.get_last_button()
# if b == self.virtual_create.Button.MoveForward:
# print("Forward pressed!")
# elif b == self.virtual_create.Button.TurnLeft:
# print("Turn Left pressed!")
# elif b == self.virtual_create.Button.TurnRight:
# print("Turn Right pressed!")
# elif b == self.virtual_create.Button.Sense:
# print("Sense pressed!")
#
# self.time.sleep(0.01)
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
# This is an example on how to detect that a button was pressed in V-REP
while True:
self.draw_particles()
b = self.virtual_create.get_last_button()
if b == self.virtual_create.Button.MoveForward:
self.filter.move(0, 0.5)
# 100 mm/s = 0.1 m/s
self.create.drive_direct(self.base_speed, self.base_speed)
self.sleep(abs(0.5 / 0.1))
# stop
self.create.drive_direct(0, 0)
elif b == self.virtual_create.Button.TurnLeft:
turn_angle = math.pi / 2 + self.odometry.theta
turn_angle %= 2 * math.pi
self.filter.move(turn_angle, 0)
# turn left by 90 degree
self.go_to_angle(turn_angle)
elif b == self.virtual_create.Button.TurnRight:
turn_angle = -math.pi / 2 + self.odometry.theta
turn_angle %= 2 * math.pi
self.filter.move(turn_angle, 0)
# turn right by 90 degree
self.go_to_angle(turn_angle)
elif b == self.virtual_create.Button.Sense:
self.filter.sense(self.sonar.get_distance())
self.sleep(0.01)
def go_to_angle(self, goal_theta):
while abs(
math.atan2(math.sin(goal_theta - self.odometry.theta),
math.cos(goal_theta - self.odometry.theta))) > 0.01:
print("Go TO: " + str(math.degrees(goal_theta)) + " " +
str(math.degrees(self.odometry.theta)))
output_theta = self.pidTheta.update(self.odometry.theta,
goal_theta, self.time.time())
self.create.drive_direct(int(+output_theta), int(-output_theta))
self.sleep(0.01)
self.create.drive_direct(0, 0)
def sleep(self, time_in_sec):
"""Sleeps for the specified amount of time while keeping odometry up-to-date
Args:
time_in_sec (float): time to sleep in seconds
"""
start = self.time.time()
while True:
state = self.create.update()
if state is not None:
self.odometry.update(state.leftEncoderCounts,
state.rightEncoderCounts)
# print("[{},{},{}]".format(self.odometry.x, self.odometry.y, math.degrees(self.odometry.theta)))
t = self.time.time()
if start + time_in_sec <= t:
break
def draw_particles(self):
data = []
# get position data from all particles
for particle in self.filter.particles:
data.extend([particle.x, particle.y, 0.1, particle.theta])
# paint all particles in simulation
self.virtual_create.set_point_cloud(data)