def fn(robot, body, pose, grasp):
joints = get_base_joints(robot)
robot_pose = multiply(pose.value, invert(grasp.value))
base_values = base_values_from_pose(robot_pose)
conf = Conf(robot, joints, base_values)
conf.assign()
if any(pairwise_collision(robot, obst) for obst in problem.obstacles):
return None
return (conf, )
def fn(robot, body, pose, grasp):
# TODO: reverse into the pick or place for differential drive
joints = get_base_joints(robot)
robot_pose = multiply(pose.value, invert(grasp.value))
base_values = base_values_from_pose(robot_pose)
conf = Conf(robot, joints, base_values)
conf.assign()
if any(pairwise_collision(robot, obst) for obst in problem.obstacles):
return None
return (conf, )
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(o, p):
if o in task.rooms: # TODO: predicate instead
return
# default_conf = arm_conf(a, g.carry)
# joints = get_arm_joints(robot, a)
# TODO: check collisions with fixed links
target_point = point_from_pose(p.value)
base_generator = visible_base_generator(robot, target_point,
base_range)
while True:
set_pose(o, p.value) # p.assign()
bq = Conf(robot, base_joints, next(base_generator))
# bq = Pose(robot, get_pose(robot))
bq.assign()
if any(pairwise_collision(robot, b) for b in fixed):
yield None
else:
yield (bq, )
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)