def gen(robot, body):
link = link_from_name(robot, BASE_LINK)
with BodySaver(robot):
set_base_conf(robot, np.zeros(3))
robot_pose = get_link_pose(robot, link)
robot_aabb = get_turtle_aabb(robot)
# _, upper = robot_aabb
# radius = upper[0]
extent = get_aabb_extent(robot_aabb)
radius = max(extent[:2]) / 2.
#draw_aabb(robot_aabb)
center, (diameter, height) = approximate_as_cylinder(body)
distance = radius + diameter / 2. + epsilon
_, _, z = get_point(body) # Assuming already placed stably
for [scale] in unit_generator(d=1, use_halton=use_halton):
#theta = PI # 0 | PI
theta = random.uniform(*theta_interval)
position = np.append(distance * unit_from_theta(theta=theta),
[z]) # TODO: halton
yaw = scale * 2 * PI - PI
quat = quat_from_euler(Euler(yaw=yaw))
body_pose = (position, quat)
robot_from_body = multiply(invert(robot_pose), body_pose)
grasp = Pose(body, robot_from_body) # TODO: grasp instead of pose
if draw:
world_pose = multiply(get_link_pose(robot, link), grasp.value)
set_pose(body, world_pose)
handles = draw_pose(get_pose(body), length=1)
wait_for_user()
remove_handles(handles)
#continue
yield (grasp, )
def gen(rover, objective):
base_joints = get_base_joints(rover)
target_point = get_point(objective)
base_generator = visible_base_generator(rover, target_point,
base_range)
lower_limits, upper_limits = get_custom_limits(rover, base_joints,
custom_limits)
attempts = 0
while True:
if max_attempts <= attempts:
attempts = 0
yield None
attempts += 1
base_conf = next(base_generator)
if not all_between(lower_limits, base_conf, upper_limits):
continue
bq = Conf(rover, base_joints, base_conf)
bq.assign()
if any(pairwise_collision(rover, b) for b in obstacles):
continue
link_pose = get_link_pose(rover,
link_from_name(rover, KINECT_FRAME))
if use_cone:
mesh, _ = get_detection_cone(rover,
objective,
camera_link=KINECT_FRAME,
depth=max_range)
if mesh is None:
continue
cone = create_mesh(mesh, color=(0, 1, 0, 0.5))
local_pose = Pose()
else:
distance = get_distance(point_from_pose(link_pose),
target_point)
if max_range < distance:
continue
cone = create_cylinder(radius=0.01,
height=distance,
color=(0, 0, 1, 0.5))
local_pose = Pose(Point(z=distance / 2.))
set_pose(cone, multiply(link_pose, local_pose))
# TODO: colors corresponding to scanned object
if any(
pairwise_collision(cone, b) for b in obstacles
if b != objective and not is_placement(objective, b)):
# TODO: ensure that this works for the rover
remove_body(cone)
continue
if not reachable_test(rover, bq):
continue
print('Visibility attempts:', attempts)
y = Ray(cone, rover, objective)
yield Output(bq, y)
def gen(rock):
base_joints = get_group_joints(rover, 'base')
x, y, _ = get_point(rock)
lower_limits, upper_limits = get_custom_limits(rover, base_joints,
custom_limits)
while True:
theta = np.random.uniform(-np.pi, np.pi)
base_conf = [x, y, theta]
if not all_between(lower_limits, base_conf, upper_limits):
continue
bq = Conf(rover, base_joints, base_conf)
bq.assign()
if any(pairwise_collision(rover, b) for b in fixed):
continue
yield (bq, )
def gen(robot, body):
link = link_from_name(robot, BASE_LINK)
with BodySaver(robot):
set_base_conf(robot, np.zeros(3))
robot_pose = get_link_pose(robot, link)
robot_aabb = get_turtle_aabb(robot)
#draw_aabb(robot_aabb)
lower, upper = robot_aabb
center, (diameter, height) = approximate_as_cylinder(body)
_, _, z = get_point(body) # Assuming already placed stably
position = Point(x=upper[0] + diameter / 2., z=z)
for [scale] in halton_generator(d=1):
yaw = scale * 2 * np.pi - np.pi
quat = quat_from_euler(Euler(yaw=yaw))
body_pose = (position, quat)
robot_from_body = multiply(invert(robot_pose), body_pose)
grasp = Pose(body, robot_from_body)
yield (grasp, )
def gen(rover, rock):
base_joints = get_base_joints(rover)
x, y, _ = get_point(rock)
lower_limits, upper_limits = get_custom_limits(rover, base_joints,
custom_limits)
while True:
for _ in range(max_attempts):
theta = np.random.uniform(-np.pi, np.pi)
base_conf = [x, y, theta]
if not all_between(lower_limits, base_conf, upper_limits):
continue
bq = Conf(rover, base_joints, base_conf)
bq.assign()
if any(pairwise_collision(rover, b) for b in obstacles):
continue
if not reachable_test(rover, bq):
continue
yield Output(bq)
break
else:
yield None
def gen(objective):
base_joints = get_group_joints(robot, 'base')
head_joints = get_group_joints(robot, 'head')
target_point = get_point(objective)
base_generator = visible_base_generator(robot, target_point,
base_range)
lower_limits, upper_limits = get_custom_limits(robot, base_joints,
custom_limits)
while True:
base_conf = next(base_generator)
if not all_between(lower_limits, base_conf, upper_limits):
continue
bq = Conf(robot, base_joints, base_conf)
bq.assign()
if any(pairwise_collision(robot, b) for b in fixed):
continue
head_conf = inverse_visibility(robot, target_point)
if head_conf is None: # TODO: test if visible
continue
hq = Conf(robot, head_joints, head_conf)
y = None
yield (bq, hq, y)
def blocked(arm='left', grasp_type='side', num=1):
x_extent = 10.0
base_limits = (-x_extent / 2. * np.ones(2), x_extent / 2. * np.ones(2))
block_width = 0.07
#block_height = 0.1
block_height = 2 * block_width
#block_height = 0.2
plate_height = 0.001
table_x = (x_extent - 1) / 2.
other_arm = get_other_arm(arm)
initial_conf = get_carry_conf(arm, grasp_type)
add_data_path()
floor = load_pybullet("plane.urdf")
pr2 = create_pr2()
set_arm_conf(pr2, arm, initial_conf)
open_arm(pr2, arm)
set_arm_conf(pr2, other_arm, arm_conf(other_arm, REST_LEFT_ARM))
close_arm(pr2, other_arm)
set_group_conf(
pr2, 'base',
[x_extent / 4, 0, 0]) # Be careful to not set the pr2's pose
table1 = create_table()
set_point(table1, Point(x=+table_x, y=0))
table2 = create_table()
set_point(table2, Point(x=-table_x, y=0))
#table3 = create_table()
#set_point(table3, Point(x=0, y=0))
plate = create_box(0.6, 0.6, plate_height, color=GREEN)
x, y, _ = get_point(table1)
plate_z = stable_z(plate, table1)
set_point(plate, Point(x=x, y=y - 0.3, z=plate_z))
#surfaces = [table1, table2, table3, plate]
surfaces = [table1, table2, plate]
green1 = create_box(block_width, block_width, block_height, color=BLUE)
green1_z = stable_z(green1, table1)
set_point(green1, Point(x=x, y=y + 0.3, z=green1_z))
# TODO: can consider a fixed wall here instead
spacing = 0.15
#red_directions = [(-1, 0), (+1, 0), (0, -1), (0, +1)]
red_directions = [(-1, 0)]
#red_directions = []
red_bodies = []
for red_direction in red_directions:
red = create_box(block_width, block_width, block_height, color=RED)
red_bodies.append(red)
x, y = get_point(green1)[:2] + spacing * np.array(red_direction)
z = stable_z(red, table1)
set_point(red, Point(x=x, y=y, z=z))
wall1 = create_box(0.01, 2 * spacing, block_height, color=GREY)
wall2 = create_box(spacing, 0.01, block_height, color=GREY)
wall3 = create_box(spacing, 0.01, block_height, color=GREY)
z = stable_z(wall1, table1)
x, y = get_point(green1)[:2]
set_point(wall1, Point(x=x + spacing, y=y, z=z))
set_point(wall2, Point(x=x + spacing / 2, y=y + spacing, z=z))
set_point(wall3, Point(x=x + spacing / 2, y=y - spacing, z=z))
green_bodies = [
create_box(block_width, block_width, block_height, color=BLUE)
for _ in range(num)
]
body_types = [(b, 'green')
for b in [green1] + green_bodies] # + [(table1, 'sink')]
movable = [green1] + green_bodies + red_bodies
initial_surfaces = {block: table2 for block in green_bodies}
sample_placements(initial_surfaces)
return Problem(
robot=pr2,
movable=movable,
arms=[arm],
grasp_types=[grasp_type],
surfaces=surfaces,
#sinks=[table1],
#goal_holding=[(arm, '?green')],
#goal_cleaned=['?green'],
goal_on=[('?green', plate)],
body_types=body_types,
base_limits=base_limits,
costs=True)