Spaces:
Sleeping
Sleeping
File size: 5,199 Bytes
1ec9b78 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 |
import numpy as np
from perception_utils import parse_query_obj
from plan_utils import get_empty_affordance_map, set_voxel_by_radius, cm2index
# Query: a point 10cm in front of [10, 15, 60].
affordance_map = get_empty_affordance_map()
# 10cm in front of so we add to x-axis
x = 10 + cm2index(10, 'x')
y = 15
z = 60
affordance_map[x, y, z] = 1
ret_val = affordance_map
# Query: a point on the right side of the table.
affordance_map = get_empty_affordance_map()
table = parse_query_obj('table')
(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
center_x, center_y, center_z = table.position
# right side so y = max_y
x = center_x
y = max_y
z = center_z
affordance_map[x, y, z] = 1
ret_val = affordance_map
# Query: a point 20cm on top of the container.
affordance_map = get_empty_affordance_map()
container = parse_query_obj('container')
(min_x, min_y, min_z), (max_x, max_y, max_z) = container.aabb
center_x, center_y, center_z = container.position
# 20cm on top of so we add to z-axis
x = center_x
y = center_y
z = max_z + cm2index(20, 'z')
affordance_map[x, y, z] = 1
ret_val = affordance_map
# Query: a point 1cm to the left of the brown block.
affordance_map = get_empty_affordance_map()
brown_block = parse_query_obj('brown block')
(min_x, min_y, min_z), (max_x, max_y, max_z) = brown_block.aabb
center_x, center_y, center_z = brown_block.position
# 1cm to the left of so we subtract from y-axis
x = center_x
y = min_y - cm2index(1, 'y')
z = center_z
affordance_map[x, y, z] = 1
ret_val = affordance_map
# Query: anywhere within 20cm of the right most block.
affordance_map = get_empty_affordance_map()
right_most_block = parse_query_obj('the right most block')
set_voxel_by_radius(affordance_map, right_most_block.position, radius_cm=20, value=1)
# Query: a point on the back side of the table.
affordance_map = get_empty_affordance_map()
table = parse_query_obj('table')
(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
center_x, center_y, center_z = table.position
# back side so x = min_x
x = min_x
y = center_y
z = center_z
affordance_map[x, y, z] = 1
ret_val = affordance_map
# Query: a point on the front right corner of the table.
affordance_map = get_empty_affordance_map()
table = parse_query_obj('table')
(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
center_x, center_y, center_z = table.position
# front right corner so x = max_x and y = max_y
x = max_x
y = max_y
z = center_z
affordance_map[x, y, z] = 1
ret_val = affordance_map
# Query: a point 30cm into the topmost drawer handle.
affordance_map = get_empty_affordance_map()
top_handle = parse_query_obj('topmost drawer handle')
# negative normal because we are moving into the handle.
moving_dir = -top_handle.normal
affordance_xyz = top_handle.position + cm2index(30, moving_dir)
affordance_map[affordance_xyz[0], affordance_xyz[1], affordance_xyz[2]] = 1
ret_val = affordance_map
# Query: a point 5cm above the blue block.
affordance_map = get_empty_affordance_map()
blue_block = parse_query_obj('blue block')
(min_x, min_y, min_z), (max_x, max_y, max_z) = blue_block.aabb
center_x, center_y, center_z = blue_block.position
# 5cm above so we add to z-axis
x = center_x
y = center_y
z = max_z + cm2index(5, 'z')
affordance_map[x, y, z] = 1
ret_val = affordance_map
# Query: a point 20cm away from the leftmost block.
affordance_map = get_empty_affordance_map()
leftmost_block = parse_query_obj('leftmost block')
# positive normal because we are moving away from the block.
moving_dir = leftmost_block.normal
affordance_xyz = leftmost_block.position + cm2index(20, moving_dir)
affordance_map[affordance_xyz[0], affordance_xyz[1], affordance_xyz[2]] = 1
ret_val = affordance_map
# Query: a point 4cm to the left of and 10cm on top of the tray that contains the lemon.
affordance_map = get_empty_affordance_map()
tray_with_lemon = parse_query_obj('tray that contains the lemon')
(min_x, min_y, min_z), (max_x, max_y, max_z) = tray_with_lemon.aabb
center_x, center_y, center_z = tray_with_lemon.position
# 4cm to the left of so we subtract from y-axis, and 10cm on top of so we add to z-axis
x = center_x
y = min_y - cm2index(4, 'y')
z = max_z + cm2index(10, 'z')
affordance_map[x, y, z] = 1
ret_val = affordance_map
# Query: a point 10cm to the right of [45 49 66], and 5cm above it.
affordance_map = get_empty_affordance_map()
# 10cm to the right of so we add to y-axis, and 5cm above it so we add to z-axis
x = 45
y = 49 + cm2index(10, 'y')
z = 66 + cm2index(5, 'z')
affordance_map[x, y, z] = 1
ret_val = affordance_map
# Query: the blue circle.
affordance_map = get_empty_affordance_map()
blue_circle = parse_query_obj('blue circle')
affordance_map = blue_circle.occupancy_map
ret_val = affordance_map
# Query: a point 10cm above and 5cm to the left of the yellow bowl.
affordance_map = get_empty_affordance_map()
yellow_bowl = parse_query_obj('yellow bowl')
(min_x, min_y, min_z), (max_x, max_y, max_z) = yellow_bowl.aabb
center_x, center_y, center_z = yellow_bowl.position
# 10cm above so we add to z-axis, and 5cm to the left of so we subtract from y-axis
x = center_x
y = min_y - cm2index(5, 'y')
z = max_z + cm2index(10, 'z')
affordance_map[x, y, z] = 1
ret_val = affordance_map
|