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