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.arm = factory.create_kuka_lbr4p()
self.virtual_create = factory.create_virtual_create()
# self.virtual_create = factory.create_virtual_create("192.168.1.XXX")
self.odometry = odometry.Odometry()
self._map = lab8_map.Map("lab8_map.json")
# self.map = lab10_map.Map("configuration_space.png")
self.map = lab10_map.Map("config2.png")
self.rrt = rrt.RRT(self.map)
self.min_dist_to_localize = 0.1
self.min_theta_to_localize = math.pi / 4
# TODO identify good PID controller gains
self.pidTheta = pid_controller.PIDController(300,
5,
50, [-10, 10],
[-200, 200],
is_angle=True)
# self.pidTheta = pid_controller.PIDController(200, 0, 100, [-10, 10], [-50, 50], is_angle=True)
# self.pidDistance = pid_controller.PIDController(300, 0, 20, [0, 0], [-200, 200], is_angle=False)
# TODO identify good particle filter parameters
self.pf = particle_filter.ParticleFilter(self._map, 5000, 0.06, 0.15,
0.2)
_ = self.create.sim_get_position()
self.my_arm_robot = MyArmRobot(factory.create_kuka_lbr4p(), self.time)
self.joint_angles = np.zeros(7)
def __init__(self, image_path, width=800, height=600):
self.root = tk.Tk()
self.img = ImageOps.fit(Image.open(image_path), (width, height),
Image.ANTIALIAS)
# self.new_img = ImageTk.PhotoImage(Image.open(image_path))
self.new_img = Image.new('RGB', (width, height))
# self.disp_img = ImageTk.PhotoImage(Image.new('RGB', (width, height)))
self.disp_img = ImageTk.PhotoImage(self.new_img)
self.width = width
self.height = height
dims = np.array([width, height])
self.rrt = rrt.RRT(dims / 2, (0, 0), dims)
self.panel = tk.Label(self.root, image=self.disp_img)
self.panel.pack(side="bottom", fill="both", expand="yes")
self.wanderer_pos = np.array([int(c) for c in self.rrt.sample()])
self.initial_brush_radius = 40
self.wanderer_step = 12
self.steps_per_tick = 10
self.brush_radius = self.initial_brush_radius
self.prev_fill_radius = [[self.initial_brush_radius] * self.height
for _ in range(self.width)]
def main():
# Map
map = cmap.Map()
# Set Initial parameters
start = map.position_2_map(np.hstack([map.get_robot_position(), map.get_robot_orientation()]))
goal = map.position_2_map(np.array([5, 2.0, 0]))
rrt = rt.RRT(start, goal, map)
# Search Path with RRT
path = rrt.planning(animation=True)
if path is None:
print("Cannot find path")
rrt.draw_graph()
plt.show()
else:
print("found path!!")
rrt.draw_graph()
rrt.draw_path(path)
plt.pause(0.01) # Need for Mac
plt.show()
# Move Robot
rt.move_robot(path, map)
def __init__(self, image_path, width=800, height=600):
self.root = tk.Tk()
self.img = ImageOps.fit(Image.open(image_path), (width, height), Image.ANTIALIAS)
# self.new_img = ImageTk.PhotoImage(Image.open(image_path))
self.new_img = Image.new('RGB', (width, height))
# self.disp_img = ImageTk.PhotoImage(Image.new('RGB', (width, height)))
self.disp_img = ImageTk.PhotoImage(self.new_img)
self.width = width
self.height = height
self.weights = [[1]*self.height for _ in range(self.width)]
dims = np.array([width, height])
self.rrt = rrt.RRT(dims/2, (0,0), dims)
# self.canvas = tk.Canvas(self.root, width=width, height=height, borderwidth=0, highlightthickness=0, bg="white")
# self.canvas.grid()
self.panel = tk.Label(self.root, image=self.disp_img)
self.panel.pack(side="bottom", fill="both", expand="yes")
def __init__(self, factory):
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.sonar = factory.create_sonar()
self.servo = factory.create_servo()
self.odometry = odometry.Odometry()
self.pidTheta = pid_controller.PIDController(300, 5, 50, [-10, 10], [-200, 200], is_angle=True)
self.map = lab10_map.Map("lab10.png")
self.rrt = rrt.RRT(self.map)
def __init__(self, image_path, width=800, height=600):
self.root = tk.Tk()
self.photo_img = ImageOps.fit(Image.open(image_path), (width, height),
Image.ANTIALIAS)
self.photo = self.photo_img.load()
# self.new_img = ImageTk.PhotoImage(Image.open(image_path))
self.canvas_img = Image.new('RGB', (width, height))
self.canvas = self.canvas_img.load()
# self.disp_img = ImageTk.PhotoImage(Image.new('RGB', (width, height)))
self.disp_img = ImageTk.PhotoImage(self.canvas_img)
self.width = width
self.height = height
dims = np.array([width, height])
self.rrt = rrt.RRT(dims / 2, (0, 0), dims)
self.panel = tk.Label(self.root, image=self.disp_img)
self.panel.pack(side="bottom", fill="both", expand="yes")
self.s_in_stroke = False
self.s_in_box = False
self.s_is_wiping = False
self.brush_pos = None
self.brush_goal = None
self.brush_dir = None
self.brush_color = None
self.pixels_of_cur_box_filled_in = 0
self.target_area = self.width * self.height / 1.1
# self.target_area = 100
self.count_at_current = 0
self.pixels_filled_in_iter = 0
self.active_brush = brush1
self.box = sample_box_with_target_area(self.target_area,
l=0,
r=self.width,
b=0,
t=self.height)
self.pixels_filled_in_area = 0
self.wipe_h = 0
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.arm = factory.create_kuka_lbr4p()
self.virtual_create = factory.create_virtual_create()
# self.virtual_create = factory.create_virtual_create("192.168.1.XXX")
self.odometry = odometry.Odometry()
self.mapJ = lab8_map.Map("lab8_map.json")
self.map = rrt_map.Map("configuration_space.png")
self.rrt = rrt.RRT(self.map)
self.path = []
self.is_localized = False
# TODO identify good PID controller gains
self.pd_controller = pd_controller.PDController(1000, 100, -75, 75)
self.pdWF = pid_controller.PIDController(200, 50, 0, [0, 0], [-50, 50])
self.pidTheta = pid_controller.PIDController(200,
0,
100, [-10, 10], [-50, 50],
is_angle=True)
self.pidTheta2 = pid_controller.PIDController(300,
5,
50, [-10, 10],
[-200, 200],
is_angle=True)
self.pidDistance = pid_controller.PIDController(1000,
0,
50, [0, 0],
[-200, 200],
is_angle=False)
# TODO identify good particle filter parameters
self.pf = particle_filter.ParticleFilter(self.mapJ, 1000, 0.06, 0.15,
0.2)
self.joint_angles = np.zeros(7)
def try_connect_rrt(self, q, q_near):
'''
Attempt to connect q to q_near using a RRT
with start = q and goal = q_near.
'''
tree = rrt.RRT(self.rrt_samples,
self.n,
self.epsilon,
self.limits,
collision_func=self.in_collision,
connect_prob=0.1)
plan = tree.build_rrt_connect(q.state, q_near.state)
#add plan as nodes if we get a plan back
if plan is not None and len(plan) > 0:
n = q
for s in plan:
new_n = Node(s)
self.T.add_node(new_n)
self.T.add_edge(new_n, n)
n = new_n
self.T.add_edge(n, q_near)
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.arm = factory.create_kuka_lbr4p()
self.virtual_create = factory.create_virtual_create()
# self.virtual_create = factory.create_virtual_create("192.168.1.218")
self.odometry = odometry.Odometry()
self.map = lab9_map.Map("finalproject_map2.json")
# TODO identify good PID controller gains
self.pidTheta = pid_controller.PIDController(200,
0,
100, [-10, 10], [-50, 50],
is_angle=True)
# TODO identify good particle filter parameters
# self.pf = particle_filter.ParticleFilter(self.map, 1000, 0.06, 0.15, 0.2)
self.vc_x = 0
self.vc_y = 0
self.vc_theta = 0
self.pf = particle_filter.ParticleFilter(self.map, 1500, 0.06, 0.15,
0.2)
self.step_dist = 0.5
self.final_x = 0.57095
self.final_y = 1.6
# RRT
self.map2 = lab11_map.Map("finalproject_map2_config.png")
self.rrt = rrt.RRT(self.map2)
self.joint_angles = np.zeros(7)
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.arm = factory.create_kuka_lbr4p()
self.virtual_create = factory.create_virtual_create()
# self.virtual_create = factory.create_virtual_create("192.168.1.XXX")
self.odometry = odometry.Odometry()
# self.map = lab8_map.Map("lab8_map.json")
self.map = lab10_map.Map("configuration_space.png")
self.pidTheta = pid_controller.PIDController(300,
5,
50, [-10, 10],
[-200, 200],
is_angle=True)
self.pidDistance = pid_controller.PIDController(300,
0,
20, [0, 0],
[-200, 200],
is_angle=False)
# self.pidTheta = pid_controller.PIDController(200, 0, 100, [-10, 10], [-50, 50], is_angle=True)
self.rrt = rrt.RRT(self.map)
self.joint_angles = np.zeros(7)
# the position that the robot returns after localizing
self.localized_x = 0
self.localized_y = 0
self.localized_theta = math.pi / 2
def draw_path(points, graph):
obs_rgb = cv.imread('obs.png', 1)
obs_rgb = obs_rgb[0:84, 0:96, :]
obs_rgb = cv.resize(obs_rgb, (1000, 700))
for key, value in graph.items():
obs_rgb[key[1]:key[1] + 5, key[0]:key[0] + 5, 0] = 0
obs_rgb[key[1]:key[1] + 5, key[0]:key[0] + 5, 1] = 0
obs_rgb[key[1]:key[1] + 5, key[0]:key[0] + 5, 2] = 255
a = np.array(points)
for point1, point2 in zip(a, a[1:]):
cv.line(obs_rgb, tuple(point1), tuple(point2), [255, 0, 0], 2)
cv.imshow('hehe', obs_rgb)
env = gym.make('CarRacing-v0')
env.reset()
render = 0
while render < 5000:
render = render + 1
env.render()
action = [0, 0.01, 0]
print("Render no: " + str(render))
observation, reward, done, info = env.step(action)
if render >= 50:
thresh, start_point, goal_point = process_obs(observation)
RRT = rrt.RRT(thresh, start_point, goal_point)
path, graph = RRT.search()
draw_path(path, graph)
print("-----------------------------------------------------")
env.close()
# Create Obstacles and their Rectangular Plot Representation #
obstacles = []
for x, y in obstacle_locations:
obs = obstacle.Obstacle(x, y, obstacle_length, obstacle_width)
obstacles.append(obs)
obstacles_work = copy.deepcopy(obstacles)
##### Configuration Space #####
config_origin = (r, r)
config_length = length - 2 * r
config_width = width - 2 * r
# Translate Obstacles to Configuration Space and get their Rectangular Plot Representation #
obs_rects_after = []
for obs in obstacles:
obs.translate(r)
obstacles_config = copy.deepcopy(obstacles)
initial_point = (100, 100)
goal_region = [(600, 600), 50, 50]
# Create Config Space Object #
config_space = cspace.Cspace(obstacles_work, obstacles_config, (r, width - r),
(r, length - r), width, length, initial_point,
goal_region)
# tree = rrt.RRT((100,100,np.pi/2), goal_region, config_space)
tree = rrt.RRT(initial_point, goal_region, config_space)
tree.explore(show_evolution=False)
import numpy as np
import rrt
import rrt_star as rrts
import robot_sim.robots.xybot.xybot as xyb
# ====Search Path with RRT====
obstacle_list = [((5, 5), 3), ((3, 6), 3), ((3, 8), 3), ((3, 10), 3),
((7, 5), 3), ((9, 5), 3), ((10, 5), 3), ((10, 0), 3),
((10, -2), 3), ((10, -4), 3), ((15, 5), 3), ((15, 7), 3),
((15, 9), 3), ((15, 11), 3), ((0, 12), 3), ((-1, 10), 3),
((-2, 8), 3)] # [x,y,size]
# Set Initial parameters
robot = xyb.XYBot()
rrt_s = rrt.RRT(robot)
rrts_s = rrts.RRT(robot)
import time
start_conf = np.array([15, 0])
goal_conf = np.array([5, -2.5])
start_conf = np.array([0, 0])
goal_conf = np.array([5, 10])
total_t = 0
for i in range(200):
tic = time.time()
path = rrt_s.plan(start_conf=start_conf,
goal_conf=goal_conf,
obstacle_list=obstacle_list,
ext_dist=1,
rand_rate=70,
max_time=1000,
def navigate(goal_xw, goal_yw, world_w, world_h, world_map):
#pub = rospy.Publisher('chatter', String, queue_size=10)
global x
global y
global heading
global pose_init
# Create ROS node
rospy.init_node('rrt_planner')
# Subscribe to currnet pose
rospy.Subscriber('base_pose_ground_truth', Odometry, current_pose_callback)
# Publisher
velocity_publisher = rospy.Publisher('cmd_vel', Twist, queue_size=10)
# Command to publish
base_cmd = Twist()
# Read Map File
r = png.Reader(filename=world_map)
w, h, pixels, metadata = r.read_flat()
# Create quadtree
#quad = QuadTree.QuadTree(0, 0, w, h, pixels, w, None)
# Wait for current pose
rate = rospy.Rate(10)
while not pose_init and not rospy.is_shutdown():
rospy.loginfo("waiting for pose init...")
rate.sleep()
x_start, y_start = helpers.world_to_map_transform(x, y, w, h, world_w,
world_h)
x_goal, y_goal = helpers.world_to_map_transform(goal_xw, goal_yw, w, h,
world_w, world_h)
rospy.loginfo("Trying to find a path from (" + str(x) + "," + str(y) +
") to (" + str(goal_xw) + ", " + str(goal_yw) + ").")
# Create RRT
rrt_planner = rrt.RRT(x_start, y_start, x_goal, y_goal, pixels, w, h)
path = rrt_planner.calculate_path(20000)
#path = QuadTree.dijkstras(x_start, y_start, x_goal, y_goal, quad)
if path == None:
rospy.loginfo("No path found.")
return
rospy.loginfo("Path found... Let's go!")
# Render map
rrt_planner.draw()
world_path = helpers.transform_path(path, w, h, world_w, world_h)
#world_path.append((goal_xw, goal_yw))
waypoint_idx = 0
# Let's go
while not rospy.is_shutdown():
dx = world_path[waypoint_idx][0] - x
dy = world_path[waypoint_idx][1] - y
if (dx * dx + dy * dy < 0.1) and (waypoint_idx < len(world_path) - 1):
waypoint_idx += 1
target_heading = math.atan2(dy, dx)
d_heading = (target_heading - heading)
if (target_heading > math.pi / 2
and heading < -math.pi / 2) or (target_heading < -math.pi / 2
and heading > math.pi / 2):
d_heading = -d_heading
# Drive forward if we are facing the right direction
if math.fabs(d_heading) < math.pi / 10 and dx * dx + dy * dy > 0.1:
base_cmd.linear.x = 2
else:
base_cmd.linear.x = 0
base_cmd.angular.z = 1 * d_heading
velocity_publisher.publish(base_cmd)
# velocity_publisher.publish(base_cmd)
rate.sleep()
#!/usr/bin/env python
import rrt
from problem import ObstacleProblem
if __name__ == '__main__':
# Problem
problem = ObstacleProblem()
# Solve
solver = rrt.RRT(problem)
final_state, tree = solver.build_rrt(problem.x_init,
problem.x_goal,
500,
0.1,
show_vis=True)
def run(self):
self.create.start()
self.create.safe()
self.servo.go_to(0)
self.create.drive_direct(0, 0)
# self.arm.open_gripper()
self.time.sleep(4)
# self.arm.close_gripper()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
self.sleep(0.5)
self.visualize()
# self.virtual_create.enable_buttons()
self.lookAround()
while True:
distance = self.sonar.get_distance()
# print(distance)
self.pf.measure(distance, 0)
if self.visualize():
break
if distance > self.step_dist + 0.2:
self.forward()
if self.lookAround():
break
else:
self.go_to_angle(self.odometry.theta + math.pi / 2)
if self.visualize():
break
if self.sonar.get_distance() < 0.4:
while True:
self.go_to_angle(self.odometry.theta + math.pi / 2)
if self.visualize() and self.sonar.get_distance() >= 0.4:
break
# state = self.create.update()
# self.odometry.update(state.leftEncoderCounts, state.rightEncoderCounts)
print("Turn left!")
print("Finished locolization!")
print(self.odometry.x, self.odometry.y, self.odometry.theta)
print(self.vc_x, self.vc_y, self.vc_theta)
self.odometry.x = self.vc_x
self.odometry.y = self.vc_y
self.odometry.theta = self.vc_theta
base_speed = 100
self.vc_x *= 100
self.vc_y = 300 - self.vc_y * 100
print(self.vc_x, self.vc_y)
self.rrt.build((self.vc_x, self.vc_y), 1500, 30)
x_goal = self.rrt.nearest_neighbor((74.095, 160))
path = self.rrt.shortest_path(x_goal)
print(len(self.rrt.T))
for v in self.rrt.T:
# print(v.state)
for u in v.neighbors:
self.map2.draw_line((v.state[0], v.state[1]),
(u.state[0], u.state[1]), (0, 0, 0))
for idx in range(0, len(path) - 1):
self.map2.draw_line(
(path[idx].state[0], path[idx].state[1]),
(path[idx + 1].state[0], path[idx + 1].state[1]), (0, 255, 0))
self.map2.save("finalproject_img.png")
# execute the path (essentially waypoint following from lab 6)
# count = 0
self.pf.set_param(0.02, 0.05, 0.1)
while math.sqrt(
math.pow(self.odometry.x - self.final_x, 2) +
math.pow(self.odometry.y - self.final_y, 2)) > 0.05:
# print("Outer loop", math.sqrt(math.pow(self.odometry.x - self.final_x, 2) + math.pow(self.odometry.y - self.final_y, 2)))
for i in range(1, len(path)):
# print("Inner loop", math.sqrt(math.pow(self.odometry.x - self.final_x, 2) + math.pow(self.odometry.y - self.final_y, 2)))
old_x = self.odometry.x
old_y = self.odometry.y
old_theta = self.odometry.theta
if math.sqrt(
math.pow(self.odometry.x - self.final_x, 2) +
math.pow(self.odometry.y - self.final_y, 2)) <= 0.05:
break
elif math.sqrt(
math.pow(self.odometry.x - self.final_x, 2) +
math.pow(self.odometry.y - self.final_y, 2)) <= 0.3:
goal_x = self.final_x
goal_y = self.final_y
goal_theta = math.atan2(goal_y - self.odometry.y,
goal_x - self.odometry.x)
self.go_to_angle(goal_theta)
while True:
state = self.create.update()
if state is not None:
self.odometry.update(state.leftEncoderCounts,
state.rightEncoderCounts)
goal_theta = math.atan2(goal_y - self.odometry.y,
goal_x - self.odometry.x)
output_theta = self.pidTheta.update(
self.odometry.theta, goal_theta,
self.time.time())
self.create.drive_direct(
int(base_speed + output_theta),
int(base_speed - output_theta))
distance = math.sqrt(
math.pow(goal_x - self.odometry.x, 2) +
math.pow(goal_y - self.odometry.y, 2))
if distance <= 0.05:
break
self.pf.move_by(self.odometry.x - old_x,
self.odometry.y - old_y,
self.odometry.theta - old_theta)
# self.visualize()
self.go_to_angle(math.pi)
self.visualize()
break
# if i == len(path)-2:
# i += 1
p = path[i]
# print(p.state)
goal_x = p.state[0] / 100.0
goal_y = 3 - p.state[1] / 100.0
# goal_theta = math.atan2(goal_y - self.odometry.y, goal_x - self.odometry.x)
# self.go_to_angle(goal_theta)
print("Goal", goal_x, goal_y)
while True:
state = self.create.update()
if state is not None:
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.theta), math.cos(self.odometry.theta))
output_theta = self.pidTheta.update(
self.odometry.theta, goal_theta, self.time.time())
print("Odometry:", self.odometry.x, self.odometry.y,
self.odometry.theta)
print("Goal:", goal_x, goal_y, goal_theta)
self.create.drive_direct(
int(base_speed + output_theta),
int(base_speed - output_theta))
# print("[{},{},{}]".format(self.odometry.x, self.odometry.y, math.degrees(self.odometry.theta)))
distance = math.sqrt(
math.pow(goal_x - self.odometry.x, 2) +
math.pow(goal_y - self.odometry.y, 2))
if i is not len(path) - 1:
if distance <= 0.2:
break
else:
if distance <= 0.01:
break
self.pf.move_by(self.odometry.x - old_x,
self.odometry.y - old_y,
self.odometry.theta - old_theta)
self.visualize()
# check rebuild the tree
if i % 5 == 0:
self.create.drive_direct(0, 0)
if not self.lookAround():
while True:
distance = self.sonar.get_distance()
print(distance)
self.pf.measure(distance, 0)
if self.visualize():
break
if distance > self.step_dist + 0.2:
self.forward()
if self.lookAround():
break
else:
self.go_to_angle(self.odometry.theta +
math.pi / 2)
if self.visualize():
break
if math.sqrt(
math.pow(self.odometry.x - self.vc_x, 2) +
math.pow(self.odometry.y - self.vc_y, 2)) > 0.1:
print("Drifted too much!")
self.odometry.x = self.vc_x
self.odometry.y = self.vc_y
self.odometry.theta = self.vc_theta
self.vc_x *= 100
self.vc_y = 300 - self.vc_y * 100
print(self.vc_x, self.vc_y)
self.map2 = lab11_map.Map(
"finalproject_map2_config.png")
self.rrt = rrt.RRT(self.map2)
self.rrt.build((self.vc_x, self.vc_y), 1500, 20)
print("New map built")
x_goal = self.rrt.nearest_neighbor((74.095, 160))
path = self.rrt.shortest_path(x_goal)
for v in self.rrt.T:
# print(v.state)
for u in v.neighbors:
self.map2.draw_line((v.state[0], v.state[1]),
(u.state[0], u.state[1]),
(0, 0, 0))
for idx in range(0, len(path) - 1):
self.map2.draw_line(
(path[idx].state[0], path[idx].state[1]),
(path[idx + 1].state[0],
path[idx + 1].state[1]), (0, 255, 0))
self.map2.save("finalproject_img.png")
break
self.create.drive_direct(0, 0)
groundTruth = self.create.sim_get_position()
print("{},{},{},{},{}".format(self.odometry.x, self.odometry.y,
self.odometry.theta * 180 / math.pi,
groundTruth[0], groundTruth[1]))
print(
"Error", self.odometry.x - self.final_x,
self.odometry.y - self.final_y,
math.sqrt(
math.pow(self.odometry.x - self.final_x, 2) +
math.pow(self.odometry.y - self.final_y, 2)))
# go to pick up glass
self.arm.open_gripper()
self.time.sleep(1)
self.arm.go_to(1, self.final_y)
self.time.sleep(3)
self.arm.go_to(3, .15)
self.time.sleep(3)
self.arm.go_to(5, 0)
self.time.sleep(3)
self.arm.close_gripper()
self.time.sleep(5)
# move over wall toward shelf
for x in range(0, 20):
self.arm.go_to(1, ((160 - x) / 100))
self.time.sleep(1)
self.arm.go_to(3, (20 - x) / 133)
self.time.sleep(0)
for y in range(0, 30):
self.arm.go_to(0, (-3.1415 * (y / 30)) / 2)
self.time.sleep(1)
# go to shelf 0
for x in range(140, 160):
self.arm.go_to(1, (x / 100))
self.time.sleep(1)
for y in range(130, 135):
self.arm.go_to(3, ((130 - y) / 100))
self.time.sleep(1)
for c in range(157, 210):
self.arm.go_to(0, -c / 100)
self.time.sleep(1)
self.arm.open_gripper()
self.time.sleep(5)
self.arm.go_to(1, self.final_y)
self.time.sleep(1)
self.arm.go_to(0, -3.1415 / 2)
self.time.sleep(1)
self.arm.go_to(1, 0)
self.arm.go_to(3, 0)
self.time.sleep(1)
#
# # go to shelf 1
# for x in range(0, 35):
# self.arm.go_to(1, ((140 - x) / 100))
# self.time.sleep(1)
# self.arm.go_to(3, x / 100)
# self.time.sleep(1)
# for c in range(157, 230):
# self.arm.go_to(0, -c / 100)
# self.time.sleep(1)
# self.arm.open_gripper()
# self.time.sleep(5)
# self.arm.go_to(1, .8)
# self.time.sleep(1)
# self.arm.go_to(0, -3.1415 / 2)
# self.time.sleep(1)
# self.arm.go_to(1, 0)
# self.arm.go_to(3, 0)
# self.time.sleep(1)
#
# # go to shelf 2
# for x in range(0, 60):
# self.arm.go_to(1, ((140 - x) / 100))
# self.time.sleep(1)
# for y in range(0, 30):
# self.arm.go_to(3, y / 100)
# self.time.sleep(1)
# for c in range(157, 230):
# self.arm.go_to(0, -c / 100)
# self.time.sleep(1)
# self.arm.open_gripper()
# self.time.sleep(5)
# self.arm.go_to(1, .6)
# self.time.sleep(1)
# self.arm.go_to(0, -3.1415 / 2)
# self.time.sleep(1)
# self.arm.go_to(1, 0)
# self.arm.go_to(3, 0)
# self.time.sleep(1)
print("finished!")
time.sleep(10)