def init(self, env: FinalEnv):
self.phase = 0
self.drive = 0
meta = env.get_metadata()
self.box_ids = meta['box_ids']
r1, r2, c1, c2, c3, c4 = env.get_agents()
self.ps = [1000, 800, 600, 600, 200, 200, 100]
self.ds = [1000, 800, 600, 600, 200, 200, 100]
r1.configure_controllers(self.ps, self.ds)
r2.configure_controllers(self.ps, self.ds)
def init(self, env: FinalEnv):
"""called before the first step, this function should also reset the state of
your solution class to prepare for the next run
"""
meta = env.get_metadata()
self.box_ids = meta['box_ids']
self.bin_id = meta['bin_id']
robot_left, robot_right, c1, c2, c3, c4 = env.get_agents()
self.spade_id = robot_right.get_metadata()['link_ids'][-1]
self.ps = [1000, 800, 600, 600, 200, 200, 100]
self.ds = [1000, 800, 600, 600, 200, 200, 100]
robot_left.configure_controllers(self.ps, self.ds)
robot_right.configure_controllers(self.ps, self.ds)
self.box_num = self.check_box(c2)
self.top_view, self.front_view = self.get_bin(c1, c2, c3, c4)
def init(self, env: FinalEnv):
self.phase = 0
self.drive = 0
self.env = env
meta = env.get_metadata()
self.box_ids = meta['box_ids']
r1, r2, c1, c2, c3, c4 = env.get_agents()
self.ps = [1000, 800, 600, 600, 200, 200, 100]
self.ds = [1000, 800, 600, 600, 200, 200, 100]
r1.configure_controllers(self.ps, self.ds)
r2.configure_controllers(self.ps, self.ds)
# get the box location from the overhead camera
# measure the bin
self.bin_id = meta['bin_id']
self.basic_info = {}
self.locate_bin_bbox(c4)
self.measured = False
def act(self, env: FinalEnv, current_timestep: int):
r1, r2, c1, c2, c3, c4 = env.get_agents()
pf_left = f = r1.get_compute_functions()['passive_force'](True, True,
False)
pf_right = f = r2.get_compute_functions()['passive_force'](True, True,
False)
if self.phase == 0:
t1 = [2, 1, 0, -1.5, -1, 1, -2]
t2 = [-2, 1, 0, -1.5, 1, 1, -2]
r1.set_action(t1, [0] * 7, pf_left)
r2.set_action(t2, [0] * 7, pf_right)
if np.allclose(r1.get_observation()[0],
t1, 0.05, 0.05) and np.allclose(
r2.get_observation()[0], t2, 0.05, 0.05):
self.phase = 1
self.counter = 0
self.selected_x = None
if self.phase == 1:
self.counter += 1
if (self.counter == 1):
selected = self.pick_box(c4)
self.selected_x = selected[0]
if self.selected_x is None:
return False
target_pose_left = Pose([self.selected_x, 0.5, 0.67],
euler2quat(np.pi, -np.pi / 3, -np.pi / 2))
self.diff_drive(r1, 9, target_pose_left)
target_pose_right = Pose([self.selected_x, -0.5, 0.6],
euler2quat(np.pi, -np.pi / 3, np.pi / 2))
self.diff_drive(r2, 9, target_pose_right)
if self.counter == 2000 / 5:
self.phase = 2
self.counter = 0
pose = r1.get_observation()[2][9]
p, q = pose.p, pose.q
p[1] = 0.07
self.pose_left = Pose(p, q)
pose = r2.get_observation()[2][9]
p, q = pose.p, pose.q
p[1] = -0.07
self.pose_right = Pose(p, q)
if self.phase == 2:
self.counter += 1
self.diff_drive(r1, 9, self.pose_left)
self.diff_drive(r2, 9, self.pose_right)
if self.counter == 2000 / 5:
self.phase = 3
pose = r2.get_observation()[2][9]
p, q = pose.p, pose.q
p[2] += 0.2
self.pose_right = Pose(p, q)
pose = r1.get_observation()[2][9]
p, q = pose.p, pose.q
p[1] = 0.5
q = euler2quat(np.pi, -np.pi / 2, -np.pi / 2)
self.pose_left = Pose(p, q)
self.counter = 0
if self.phase == 3:
self.counter += 1
self.diff_drive(r1, 9, self.pose_left)
self.diff_drive(r2, 9, self.pose_right)
if self.counter == 200 / 5:
self.phase = 4
self.counter = 0
if self.phase == 4:
self.counter += 1
if (self.counter < 3000 / 5):
pose = r2.get_observation()[2][9]
p, q = pose.p, pose.q
q = euler2quat(np.pi, -np.pi / 1.5, quat2euler(q)[2])
self.diff_drive2(r2, 9, Pose(p, q), [4, 5, 6],
[0, 0, 0, -1, 0], [0, 1, 2, 3, 4])
elif (self.counter < 6000 / 5):
pose = r2.get_observation()[2][9]
p, q = pose.p, pose.q
q = euler2quat(np.pi, -np.pi / 1.5, quat2euler(q)[2])
self.diff_drive2(r2, 9, Pose(p, q), [4, 5, 6],
[0, 0, 1, -1, 0], [0, 1, 2, 3, 4])
elif (self.counter < 9000 / 5):
p = [-1, 0, 1.5]
q = euler2quat(0, -np.pi / 1.5, 0)
self.diff_drive(r2, 9, Pose(p, q))
else:
self.phase = 0
return v
def pick_box(self, c):
color, depth, segmentation = c.get_observation()
np.random.shuffle(self.box_ids)
for i in self.box_ids:
m = np.where(segmentation == i)
if len(m[0]):
min_x = 10000
max_x = -1
min_y = 10000
max_y = -1
for y, x in zip(m[0], m[1]):
min_x = min(min_x, x)
max_x = max(max_x, x)
min_y = min(min_y, y)
max_y = max(max_y, y)
x, y = round((min_x + max_x) / 2), round((min_y + max_y) / 2)
return self.get_global_position_from_camera(c, depth, x, y)
return False
if __name__ == '__main__':
np.random.seed(0)
env = FinalEnv()
# env.run(Solution(), render=True, render_interval=5, debug=True)
env.run(Solution())
env.close()
def act(self, env: FinalEnv, current_timestep: int):
"""called at each (actionable) time step to set robot actions. return False to
indicate that the agent decides to move on to the next environment.
Returning False early could mean a lower success rate (correctly placed
boxes / total boxes), but it can also save you some time, so given a
fixed total time budget, you may be able to place more boxes.
"""
# robot_left, robot_right, camera_front, camera_left, camera_right, camera_top = env.get_agents()
robot_left, robot_right, c1, c2, c3, c4 = env.get_agents()
pf_left = f = robot_left.get_compute_functions()['passive_force'](True, True, False)
pf_right = f = robot_right.get_compute_functions()['passive_force'](True, True, False)
if self.phase == 0:
self.counter += 1
t1 = [2, 1, 0, -1.5, -1, 1, -2.7]
t2 = [-2, 1, 0, -1.5, 1, 1, -2]
robot_left.set_action(t1, [0] * 7, pf_left)
robot_right.set_action(t2, [0] * 7, pf_right)
if np.allclose(robot_left.get_observation()[0], t1, 0.05, 0.05) and np.allclose(
robot_right.get_observation()[0], t2, 0.05, 0.05):
self.phase = 1
self.counter = 0
self.selected_x = None
self.spade_width, self.spade_length = self.get_spade(c4)
print(self.spade_width, self.spade_length)
if (self.counter > 8000/5):
self.counter = 0
return False
if self.phase == 1:
# print(11111)
self.counter += 1
if (self.counter == 1):
self.selected_x, self.selected_z = self.pick_box(c4)
print(self.selected_x,self.selected_z)
if self.selected_x is None:
self.counter = 0
self.phase = 0
return False
# change 0.67 to 0.69
target_pose_left = Pose([-0.3, 0.1, 0.55], euler2quat(np.pi, -np.pi / 3, -np.pi/2 ))
self.diff_drive(robot_left, 9, target_pose_left)
target_pose_right = Pose([-0.3, 0.1, 0.55], euler2quat(np.pi, -np.pi / 3, np.pi/2))
self.diff_drive(robot_right, 9, target_pose_right)
if self.counter == 2000 / 5:
self.phase = 2
self.counter = 0
pose = robot_left.get_observation()[2][9]
p, q = pose.p, pose.q
p[0] = 1
self.pose_left = Pose(p, q)
pose = robot_right.get_observation()[2][9]
p, q = pose.p, pose.q
p[0] = 1
self.pose_right = Pose(p, q)
if self.phase == 2:
# print(22222)
self.counter += 1
self.diff_drive(robot_left, 9, self.pose_left)
self.diff_drive(robot_right, 9, self.pose_right)
if self.counter == 600 / 5:
self.phase = 3
self.counter = 0
if self.phase == 3:
self.counter += 1
if (self.counter < 400/5):
pose = robot_left.get_observation()[2][9]
p, q = pose.p, pose.q
p[0] = -3
self.diff_drive(robot_left, 9, Pose(p, q))
pose = robot_right.get_observation()[2][9]
p, q = pose.p, pose.q
p[0] = -3
self.diff_drive(robot_right, 9, Pose(p, q))
else:
t1 = [2, 1, 0, -1.5, -1, 1, -2]
t2 = [-2, 1, 0, -1.5, 1, 1, -2]
robot_left.set_action(t1, [0] * 7, pf_left)
robot_right.set_action(t2, [0] * 7, pf_right)
if np.allclose(robot_left.get_observation()[0], t1, 0.05, 0.05) and np.allclose(
robot_right.get_observation()[0], t2, 0.05, 0.05):
self.phase = 4
self.counter = 0
self.selected_x = None
self.spade_width, self.spade_length = self.get_spade(c4)
print(self.spade_width, self.spade_length)
if (self.counter > 8000/5):
self.counter = 0
return False
if self.phase == 4:
# print(11111)
self.counter += 1
if (self.counter == 1):
self.selected_x, self.selected_z = self.pick_box(c4)
print(self.selected_x,self.selected_z)
if self.selected_x is None:
self.counter = 0
self.phase = 0
return False
# change 0.67 to 0.69
target_pose_left = Pose([self.selected_x, 0.6, self.selected_z + 0.65], euler2quat(np.pi, -np.pi / 3, -np.pi / 2))
self.diff_drive(robot_left, 9, target_pose_left)
target_pose_right = Pose([self.selected_x, -0.6, self.selected_z + 0.55], euler2quat(np.pi, -np.pi / 3, np.pi / 2))
self.diff_drive(robot_right, 9, target_pose_right)
if self.counter == 1500 / 5:
self.phase = 5
self.counter = 0
pose = robot_left.get_observation()[2][9]
p, q = pose.p, pose.q
p[1] = self.spade_length/2
self.pose_left = Pose(p, q)
pose = robot_right.get_observation()[2][9]
p, q = pose.p, pose.q
p[1] = - self.spade_length / 2
self.pose_right = Pose(p, q)
if self.phase == 5:
# print(22222)
self.counter += 1
self.diff_drive(robot_left, 9, self.pose_left)
self.diff_drive(robot_right, 9, self.pose_right)
if self.counter == 2500 / 5:
self.phase = 6
pose = robot_right.get_observation()[2][9]
p, q = pose.p, pose.q
# p[2] += 0.1
self.pose_right = Pose(p, q)
# pose = robot_left.get_observation()[2][9]
# p, q = pose.p, pose.q
# p[1] = -0.3
# p[2] += 0.3
# q = euler2quat(np.pi, -np.pi / 8, -np.pi / 2)
# self.pose_left = Pose(p, q)
self.counter = 0
if self.phase == 6:
# print(33333)
pose = robot_left.get_observation()[2][9]
p, q = pose.p, pose.q
# p[1] -= 0.1
p[2] += 0.1
q = euler2quat(np.pi, -np.pi / 4, -np.pi /2)
self.pose_left = Pose(p, q)
self.counter += 1
self.diff_drive(robot_left, 9, self.pose_left)
if self.counter == 200 / 5:
self.phase = 7
self.counter = 0
if self.phase == 7:
# print(self.counter)
# self.diff_drive(robot_right, 9, self.pose_right)
self.counter += 1
if (self.counter < 200 / 5):
pose = robot_left.get_observation()[2][9]
p, q = pose.p, pose.q
p[1] += 5
p[2] += 5
# q = euler2quat(np.pi, 0, -np.pi / 4)
# q = euler2quat(np.pi, -np.pi / 4, -np.pi /2)
self.diff_drive(robot_left, 9, Pose(p, q))
elif (self.counter < 300 / 5):
pose = robot_right.get_observation()[2][9]
p, q = pose.p, pose.q
q = euler2quat(0,0 ,np.pi/4)
self.diff_drive(robot_right, 9, Pose(p, q))
elif (self.counter < 6500 / 5):
p = [self.top_view[0], self.top_view[1] + self.spade_length /3, 2.0*self.front_view[2]]
q = euler2quat(0, -np.pi / 1.5, 0)
self.diff_drive(robot_right, 9, Pose(p, q))
# if self.counter == 500/5:
# cur_box = self.check_box(c2)
# print(self.box_num, cur_box)
# if (cur_box >= self.box_num):
# self.phase = 0
# return False
elif (self.counter < 9000 / 5):
pose = robot_right.get_observation()[2][9]
p = pose.p
q = euler2quat(0, -np.pi / 1.5, 0)
self.diff_drive(robot_right, 9, Pose(p, q))
else:
self.phase = 0
self.counter = 0
return False
def act(self, env: FinalEnv, current_timestep: int):
"""called at each (actionable) time step to set robot actions. return False to
indicate that the agent decides to move on to the next environment.
Returning False early could mean a lower success rate (correctly placed
boxes / total boxes), but it can also save you some time, so given a
fixed total time budget, you may be able to place more boxes.
"""
robot_left, robot_right, camera_front, camera_left, camera_right, camera_top = env.get_agents(
)
return v
def pick_box(self, c):
color, depth, segmentation = c.get_observation()
np.random.shuffle(self.box_ids)
for i in self.box_ids:
m = np.where(segmentation == i)
if len(m[0]):
min_x = 10000
max_x = -1
min_y = 10000
max_y = -1
for y, x in zip(m[0], m[1]):
min_x = min(min_x, x)
max_x = max(max_x, x)
min_y = min(min_y, y)
max_y = max(max_y, y)
x, y = round((min_x + max_x) / 2), round((min_y + max_y) / 2)
return self.get_global_position_from_camera(c, depth, x, y)
return False
if __name__ == '__main__':
np.random.seed(0)
env = FinalEnv()
# env.run(Solution(), render=True, render_interval=5, debug=True)
env.run(Solution(), render=True, render_interval=5)
env.close()
def act(self, env: FinalEnv, current_timestep: int):
global global_time_left
r1, r2, c1, c2, c3, c4 = env.get_agents()
pf_left = f = r1.get_compute_functions()['passive_force'](True, True, False)
pf_right = f = r2.get_compute_functions()['passive_force'](True, True, False)
if self.phase == 0:
if 20000 - (current_timestep // 5) < 4100:
return False
t1 = [2, 1, 0, -1.5, -1, 1, -2]
t2 = [-2, 1, 0, -1.5, 1, 1, -2]
r1.set_action(t1, [0] * 7, pf_left)
r2.set_action(t2, [0] * 7, pf_right)
pose = r1.get_observation()[2][9]
p, q = pose.p, pose.q
#print("init r1 position ",p, quat2euler(q))
if np.allclose(r1.get_observation()[0], t1, 0.05, 0.05) and np.allclose(
r2.get_observation()[0], t2, 0.05, 0.05):
self.phase = 1
self.counter = 0
self.selected_x = None
if self.phase == 1:
self.counter += 1
if (self.counter == 1):
selected, flag = self.pick_box(c4)
if 20000 - current_timestep//5 < global_time_left:
if flag == False:
return False
self.selected_x = selected[0]
self.selected_y = selected[1]
target_pose_left = Pose([self.selected_x, 0.5, 0.67], euler2quat(np.pi, -np.pi / 6, -np.pi / 2))
self.diff_drive(r1, 9, target_pose_left)
#self.jacobian_drive(r1, 9, target_pose_left)
pose = r1.get_observation()[2][9]
p, q = pose.p, pose.q
target_pose_right = Pose([self.selected_x, -0.5, 0.6], euler2quat(np.pi, -np.pi / 4, np.pi / 2))
self.diff_drive(r2, 9, target_pose_right)
#self.jacobian_drive(r2, 9, target_pose_right)
if self.counter == 2000 / 5:
self.phase = 2
self.counter = 0
pose = r1.get_observation()[2][9]
p, q = pose.p, pose.q
p[1] = 0.07
self.pose_left = Pose(p, q)
pose = r2.get_observation()[2][9]
p, q = pose.p, pose.q
p[1] = -0.07
self.pose_right = Pose(p, q)
if self.phase == 2:
self.counter += 1
self.diff_drive(r1, 9, self.pose_left)
self.diff_drive(r2, 9, self.pose_right)
if self.counter == 3000 / 5:
self.phase = 3
pose = r1.get_observation()[2][9]
p, q = pose.p, pose.q
# p[1] = 0.5
p[2] += 0.1
q = euler2quat(np.pi, -np.pi / 4, -np.pi / 2)
self.pose_left = Pose(p, q)
pose = r2.get_observation()[2][9]
p, q = pose.p, pose.q
p[2] += 0.0
q = euler2quat(np.pi, -np.pi / 1.8, np.pi / 2)
self.pose_right = Pose(p, q)
self.counter = 0
if self.phase == 3:
if self.counter < 500 / 5:
self.counter += 1
self.diff_drive(r1, 9, self.pose_left)
self.diff_drive(r2, 9, self.pose_right)
elif self.counter < 1500 / 5:
self.counter += 1
t1 = [2, 1, 0, -1.5, -1, 1, -2]
r1.set_action(t1, [0] * 7, pf_left)
self.diff_drive(r2, 9, self.pose_right)
else:
self.phase = 4
self.counter = 0
if not self.measured:
self.measure_spade(c4, r2)
if self.spade_is_empty(c4, r2):
self.phase = 0
global_time_left -= 1.
return True
if self.phase == 4:
self.counter += 1
# middle point 1
if (self.counter < 3000 / 5):
pose = r2.get_observation()[2][9]
p, q = pose.p, pose.q
p[2] += 0.5
q = euler2quat(np.pi, -np.pi / 1.5, quat2euler(q)[2])
self.jacobian_drive(r2, 9, Pose(p, q))
elif (self.counter < 9000 / 5):
# p = [-1, -0.15, 1.18] #a milestone to control the trajectory for avoiding collision
p = [-1, -0., 1.2] #a milestone to control the trajectory for avoiding collision
q = euler2quat(0, -np.pi / 3, 0)
self.jacobian_drive(r2, 9, Pose(p, q), speed=0.4)
elif (self.counter < 10000 / 5):
cent = self.basic_info['bin_center']
length = self.basic_info['spade_length']
p = cent.copy()
# p[2] += 0.1
p[0] += length * 2.
#p = [-1, -0.1, 1.2]
q = euler2quat(0, -np.pi / 1.2, 0)
self.jacobian_drive(r2, 9, Pose(p, q), speed=0.4)
elif (self.counter < 11000 / 5):
cent = self.basic_info['bin_center']
length = self.basic_info['spade_length']
p = cent.copy()
# p[2] += 0.15
p[0] += length * 2.
# p = [-1, -0.1, 1.2]
q = euler2quat(0, -np.pi / 1., 0)
self.jacobian_drive(r2, 9, Pose(p, q), speed=0.4)
else:
# selected, flag = self.pick_box(c4)
# if 20000 - current_timestep//5 < global_time_left:
# if flag == False:
# return False
self.phase = 0
global_time_left -= 1.
