def above_floor(self, states, rotation):
'''
judge all the floors are above the ground.
'''
floor_center = states[0][:3]
corner_relative = [
np.array([self.glass_dis_x / 2., -self.border / 2., self.glass_dis_z / 2.]),
np.array([self.glass_dis_x / 2., -self.border / 2., -self.glass_dis_z / 2.]),
np.array([-self.glass_dis_x / 2., -self.border / 2., self.glass_dis_z / 2.]),
np.array([-self.glass_dis_x / 2., -self.border / 2., -self.glass_dis_z / 2.]),
np.array([self.glass_dis_x / 2., self.border / 2. + self.height, self.glass_dis_z / 2.]),
np.array([self.glass_dis_x / 2., self.border / 2. + self.height, -self.glass_dis_z / 2.]),
np.array([-self.glass_dis_x / 2., self.border / 2. + self.height, self.glass_dis_z / 2.]),
np.array([-self.glass_dis_x / 2., self.border / 2. + self.height, -self.glass_dis_z / 2.]),
]
for corner_rel in corner_relative:
corner_real = rotate_rigid_object(center=floor_center, axis=np.array([0, 0, -1]), angle=rotation,
relative=corner_rel)
if corner_real[1] < - self.border:
return False
return True
def judge_glass_collide(self, new_states, rotation):
'''
judge if the front wall of the pouring glass would collide with the front wall of the poured glass.
'''
pouring_right_wall_center = new_states[2][:3]
pouring_left_wall_center = new_states[1][:3]
# build the corner of the front wall of the control glass
r_corner1_relative_cord = np.array([self.border / 2., self.height / 2., self.glass_dis_z / 2 + self.border])
r_corner1_real = rotate_rigid_object(center=pouring_right_wall_center, axis=np.array([0, 0, -1]), angle=rotation,
relative=r_corner1_relative_cord)
r_corner3_relative_cord = np.array([self.border / 2., -self.height / 2., self.glass_dis_z / 2 - self.border])
r_corner3_real = rotate_rigid_object(center=pouring_right_wall_center, axis=np.array([0, 0, -1]), angle=rotation,
relative=r_corner3_relative_cord)
r_corner5_relative_cord = np.array([-self.border / 2., -self.height / 2., self.glass_dis_z / 2 + self.border])
r_corner5_real = rotate_rigid_object(center=pouring_left_wall_center, axis=np.array([0, 0, -1]), angle=rotation,
relative=r_corner5_relative_cord)
r_corner8_relative_cord = np.array([-self.border / 2., self.height / 2., self.glass_dis_z / 2 + self.border])
r_corner8_real = rotate_rigid_object(center=pouring_left_wall_center, axis=np.array([0, 0, -1]), angle=rotation,
relative=r_corner8_relative_cord)
control_polygon = Polygon([r_corner1_real[:2], r_corner3_real[:2], r_corner5_real[:2], r_corner8_real[:2]])
left_wall_center = self.poured_glass_states[1][:3]
leftx, lefty = left_wall_center[0], left_wall_center[1]
right_wall_center = self.poured_glass_states[2][:3]
rightx, righty = right_wall_center[0], right_wall_center[1]
border = self.poured_border
target_front_corner1 = np.array([leftx - border / 2, lefty + self.poured_height / 2])
traget_front_corner2 = np.array([leftx - border / 2, lefty - self.poured_height / 2])
traget_front_corner3 = np.array([rightx + border / 2, righty - self.poured_height / 2])
target_front_corner4 = np.array([rightx + border / 2, righty + self.poured_height / 2])
target_polygon = Polygon([target_front_corner1, traget_front_corner2, traget_front_corner3, target_front_corner4])
res = control_polygon.intersects(target_polygon)
return res
def rotate_glass(self, prev_states, x, y, theta):
'''
given the previous states of the glass, rotate it with angle theta.
update the states of the 5 boxes that form the box: floor, left/right wall, back/front wall.
rotate the glass, where the center point is the center of the floor or the top.
state:
0-3: current (x, y, z) coordinate of the center point
3-6: previous (x, y, z) coordinate of the center point
6-10: current quat
10-14: previous quat
'''
dis_x, dis_z = self.glass_dis_x, self.glass_dis_z
quat_curr = quatFromAxisAngle([0, 0, -1.], theta)
border = self.border
# states of 5 walls
states = np.zeros((5, self.dim_shape_state))
for i in range(5):
states[i][3:6] = prev_states[i][:3]
states[i][10:] = prev_states[i][6:10]
x_center = x
# rotation center is the floor center
rotate_center = np.array([x_center, y, 0.])
if self.action_mode == 'rotation_bottom':
# floor: center position does not change
states[0, :3] = np.array([x_center, y, 0.])
# left wall: center must move right and move down.
relative_coord = np.array(
[-(dis_x + border) / 2., (self.height) / 2., 0.])
states[1, :3] = rotate_rigid_object(center=rotate_center,
axis=np.array([0, 0, -1]),
angle=theta,
relative=relative_coord)
# right wall
relative_coord = np.array([(dis_x + border) / 2.,
(self.height) / 2., 0.])
states[2, :3] = rotate_rigid_object(center=rotate_center,
axis=np.array([0, 0, -1]),
angle=theta,
relative=relative_coord)
# back wall
relative_coord = np.array(
[0, (self.height) / 2., -(dis_z + border) / 2.])
states[3, :3] = rotate_rigid_object(center=rotate_center,
axis=np.array([0, 0, -1]),
angle=theta,
relative=relative_coord)
# front wall
relative_coord = np.array(
[0, (self.height) / 2., (dis_z + border) / 2.])
states[4, :3] = rotate_rigid_object(center=rotate_center,
axis=np.array([0, 0, -1]),
angle=theta,
relative=relative_coord)
elif self.action_mode == 'rotation_top':
# floor
relative_coord = np.array([0, -self.height, 0.])
states[0, :3] = rotate_rigid_object(center=rotate_center,
axis=np.array([0, 0, -1]),
angle=theta,
relative=relative_coord)
# left wall
relative_coord = np.array(
[-(dis_x + border) / 2., -self.height / 2., 0.])
states[1, :3] = rotate_rigid_object(center=rotate_center,
axis=np.array([0, 0, -1]),
angle=theta,
relative=relative_coord)
# right wall
relative_coord = np.array([(dis_x + border) / 2.,
-self.height / 2., 0.])
states[2, :3] = rotate_rigid_object(center=rotate_center,
axis=np.array([0, 0, -1]),
angle=theta,
relative=relative_coord)
# back wall
relative_coord = np.array(
[0, -self.height / 2., -(dis_z + border) / 2.])
states[3, :3] = rotate_rigid_object(center=rotate_center,
axis=np.array([0, 0, -1]),
angle=theta,
relative=relative_coord)
# front wall
relative_coord = np.array([0, -self.height / 2., (dis_z) / 2.])
states[4, :3] = rotate_rigid_object(center=rotate_center,
axis=np.array([0, 0, -1]),
angle=theta,
relative=relative_coord)
states[:, 6:10] = quat_curr
return states