def test(b1, p1, g1, b2, p2):
if not collisions or (b1 == b2):
return True
p2.assign()
for _ in iterate_approach_path(problem.robot, 'left', gripper, p1, g1, body=b1):
if pairwise_collision(b1, b2) or pairwise_collision(gripper, b2):
return False
return True
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(o, p):
# 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:
for _ in range(max_attempts):
set_pose(o, p.value)
base_conf = next(base_generator)
#set_base_values(robot, base_conf)
set_joint_positions(robot, base_joints, base_conf)
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
#bq = Pose(robot, get_pose(robot))
bq = Conf(robot, base_joints, base_conf)
hq = Conf(robot, head_joints, head_conf)
yield (bq, hq)
break
else:
yield None
def test(traj1, traj2):
if not problem.collisions:
return True
if (robot1 is None) or (robot2 is None):
return True
if traj1 is traj2:
return False
if not aabb_overlap(get_turtle_traj_aabb(traj1),
get_turtle_traj_aabb(traj2)):
return True
# TODO: could also use radius to prune
# TODO: prune if start and end conf collide
for conf1 in randomize(traj1.iterate()):
if not aabb_overlap(get_turtle_traj_aabb(conf1),
get_turtle_traj_aabb(traj2)):
continue
set_base_conf(robot1, conf1.values)
for conf2 in randomize(traj2.iterate()):
if not aabb_overlap(get_turtle_traj_aabb(conf1),
get_turtle_traj_aabb(conf2)):
continue
set_base_conf(robot2, conf2.values)
if pairwise_collision(robot1, robot2):
return False
return True
def test(ray, rover, conf):
if not collisions or (rover == ray.start) or (rover == ray.end):
return True
conf.assign()
collision = pairwise_collision(ray.body, rover)
#if collision:
# wait_for_user()
return not collision
def test(c, b2, p2):
if not collisions:
return True
state = c.assign()
if b2 in state.attachments:
return True
p2.assign()
for _ in c.apply(state):
state.assign()
for b1 in state.attachments:
if pairwise_collision(b1, b2):
#wait_for_user()
return False
if pairwise_collision(problem.robot, b2):
return False
# TODO: just check collisions with moving links
return True
def test(c, a, b1, g, b2, p2):
raise NotImplementedError()
if not collisions or (b1 == b2):
return True
state = c.assign()
if (b1 in state.attachments) or (b2 in state.attachments):
return True
p2.assign()
grasp_attachment = g.get_attachment(problem.robot, a)
for _ in c.apply(state):
state.assign()
grasp_attachment.assign()
if pairwise_collision(b1, b2):
return False
if pairwise_collision(problem.robot, b2):
return False
return True
def fn(robot, conf, body, grasp):
conf.assign()
link = link_from_name(robot, BASE_LINK)
world_from_body = multiply(get_link_pose(robot, link), grasp.value)
pose = Pose(body, world_from_body)
pose.assign()
if any(pairwise_collision(body, obst) for obst in problem.obstacles):
return None
return (pose, )
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 test(b1, p1, g1, b2, p2):
if not collisions or (b1 == b2):
return True
p2.assign()
grasp_pose = multiply(p1.value, invert(g1.value))
approach_pose = multiply(p1.value, invert(g1.approach), g1.value)
for obj_pose in interpolate_poses(grasp_pose, approach_pose):
set_pose(b1, obj_pose)
if pairwise_collision(b1, b2):
return False
return True
def check_trajectory_collision(robot, trajectory, bodies):
# TODO: each new addition makes collision checking more expensive
#offset = 4
movable_joints = get_movable_joints(robot)
#for q in trajectory[offset:-offset]:
collisions = [False for _ in range(len(bodies))] # TODO: batch collision detection
for q in trajectory:
set_joint_positions(robot, movable_joints, q)
for i, body in enumerate(bodies):
if not collisions[i]:
collisions[i] |= pairwise_collision(robot, body)
return collisions
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 test(r, t, b2, p2):
if not problem.collisions:
return True
p2.assign()
state = State()
for _ in t.apply(state):
state.assign()
#for b1 in state.attachments:
# if pairwise_collision(b1, b2):
# return False
if pairwise_collision(r, b2):
set_renderer(True)
color = get_visual_data(b2)[0].rgbaColor
handles = add_line(point_from_conf(t.path[0].values, z=0.02),
point_from_conf(t.path[-1].values, z=0.02), color=color)
wait_for_user()
set_renderer(False)
return False
return True
def sample_placements(body_surfaces, obstacles=None, min_distances={}):
if obstacles is None:
obstacles = [
body for body in get_bodies() if body not in body_surfaces
]
obstacles = list(obstacles)
# TODO: max attempts here
for body, surface in body_surfaces.items():
min_distance = min_distances.get(body, 0.01)
while True:
pose = sample_placement(body, surface)
if pose is None:
return False
if not any(
pairwise_collision(body, obst, max_distance=min_distance)
for obst in obstacles if obst not in [body, surface]):
obstacles.append(body)
break
return True
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 test(r, q, b2, p2):
if not problem.collisions:
return True
q.assign()
p2.assign()
return not pairwise_collision(r, b2)
def test(b1, p1, b2, p2):
if not collisions or (b1 == b2):
return True
p1.assign()
p2.assign()
return not pairwise_collision(b1, b2, **kwargs) #, max_distance=0.001)
