def plan_water_motion(body,
joints,
end_conf,
attachment,
obstacles=None,
attachments=[],
self_collisions=True,
disabled_collisions=set(),
max_distance=MAX_DISTANCE,
weights=None,
resolutions=None,
reference_pose=unit_pose(),
custom_limits={},
**kwargs):
assert len(joints) == len(end_conf)
sample_fn = get_sample_fn(body, joints, custom_limits=custom_limits)
distance_fn = get_distance_fn(body, joints, weights=weights)
extend_fn = get_extend_fn(body, joints, resolutions=resolutions)
collision_fn = get_collision_fn(body,
joints,
obstacles, {attachment} | set(attachments),
self_collisions,
disabled_collisions,
max_distance=max_distance,
custom_limits=custom_limits)
def water_test(q):
if attachment is None:
return False
set_joint_positions(body, joints, q)
attachment.assign()
attachment_pose = get_pose(attachment.child)
pose = multiply(attachment_pose,
reference_pose) # TODO: confirm not inverted
roll, pitch, _ = euler_from_quat(quat_from_pose(pose))
violation = (MAX_ROTATION < abs(roll)) or (MAX_ROTATION < abs(pitch))
#if violation: # TODO: check whether different confs can be waypoints for each object
# print(q, violation)
# wait_for_user()
return violation
invalid_fn = lambda q, **kwargs: water_test(q) or collision_fn(q, **kwargs)
start_conf = get_joint_positions(body, joints)
if not check_initial_end(start_conf, end_conf, invalid_fn):
return None
return birrt(start_conf, end_conf, distance_fn, sample_fn, extend_fn,
invalid_fn, **kwargs)
def plan_workspace(world,
tool_path,
obstacles,
randomize=True,
teleport=False):
# Assuming that pairs of fixed things aren't in collision at this point
moving_links = get_moving_links(world.robot, world.arm_joints)
robot_obstacle = (world.robot, frozenset(moving_links))
distance_fn = get_distance_fn(world.robot, world.arm_joints)
if randomize:
sample_fn = get_sample_fn(world.robot, world.arm_joints)
set_joint_positions(world.robot, world.arm_joints, sample_fn())
else:
world.carry_conf.assign()
arm_path = []
for i, tool_pose in enumerate(tool_path):
#set_joint_positions(world.kitchen, [door_joint], door_path[i])
tolerance = INF if i == 0 else NEARBY_PULL
full_arm_conf = world.solve_inverse_kinematics(
tool_pose, nearby_tolerance=tolerance)
if full_arm_conf is None:
# TODO: this fails when teleport=True
if PRINT_FAILURES: print('Workspace kinematic failure')
return None
if any(pairwise_collision(robot_obstacle, b) for b in obstacles):
if PRINT_FAILURES: print('Workspace collision failure')
return None
arm_conf = get_joint_positions(world.robot, world.arm_joints)
if arm_path and not teleport:
distance = distance_fn(arm_path[-1], arm_conf)
if MAX_CONF_DISTANCE < distance:
if PRINT_FAILURES:
print(
'Workspace proximity failure (distance={:.5f})'.format(
distance))
return None
arm_path.append(arm_conf)
# wait_for_user()
return arm_path
def compute_motion(robot, fixed_obstacles, element_bodies,
printed_elements, start_conf, end_conf,
collisions=True, max_time=INF, buffer=0.1, smooth=100):
# TODO: can also just plan to initial conf and then shortcut
joints = get_movable_joints(robot)
assert len(joints) == len(end_conf)
weights = JOINT_WEIGHTS
resolutions = np.divide(RESOLUTION * np.ones(weights.shape), weights)
disabled_collisions = get_disabled_collisions(robot)
custom_limits = {}
#element_from_body = {b: e for e, b in element_bodies.items()}
hulls, obstacles = {}, []
if collisions:
element_obstacles = {element_bodies[e] for e in printed_elements}
obstacles = set(fixed_obstacles) | element_obstacles
#hulls, obstacles = decompose_structure(fixed_obstacles, element_bodies, printed_elements)
#print(hulls)
#print(obstacles)
#wait_for_user()
#printed_elements = set(element_bodies)
bounding = None
if printed_elements:
# TODO: pass in node_points
bounding = create_bounding_mesh(printed_elements, element_bodies=element_bodies, node_points=None,
buffer=0.01) # buffer=buffer)
#wait_for_user()
sample_fn = get_sample_fn(robot, joints, custom_limits=custom_limits)
distance_fn = get_distance_fn(robot, joints, weights=weights)
extend_fn = get_extend_fn(robot, joints, resolutions=resolutions)
#collision_fn = get_collision_fn(robot, joints, obstacles, attachments={}, self_collisions=SELF_COLLISIONS,
# disabled_collisions=disabled_collisions, custom_limits=custom_limits, max_distance=0.)
collision_fn = get_element_collision_fn(robot, obstacles)
fine_extend_fn = get_extend_fn(robot, joints, resolutions=1e-1*resolutions) #, norm=INF)
def test_bounding(q):
set_joint_positions(robot, joints, q)
collision = (bounding is not None) and pairwise_collision(robot, bounding, max_distance=buffer) # body_collision
return q, collision
def dynamic_extend_fn(q_start, q_end):
# TODO: retime trajectories to be move more slowly around the structure
for (q1, c1), (q2, c2) in get_pairs(map(test_bounding, extend_fn(q_start, q_end))):
# print(c1, c2, len(list(fine_extend_fn(q1, q2))))
# set_joint_positions(robot, joints, q2)
# wait_for_user()
if c1 and c2:
for q in fine_extend_fn(q1, q2):
# set_joint_positions(robot, joints, q)
# wait_for_user()
yield q
else:
yield q2
def element_collision_fn(q):
if collision_fn(q):
return True
#for body in get_bodies_in_region(get_aabb(robot)): # Perform per link?
# if (element_from_body.get(body, None) in printed_elements) and pairwise_collision(robot, body):
# return True
for hull, bodies in hulls.items():
if pairwise_collision(robot, hull) and any(pairwise_collision(robot, body) for body in bodies):
return True
return False
path = None
if check_initial_end(start_conf, end_conf, collision_fn):
path = birrt(start_conf, end_conf, distance_fn, sample_fn, dynamic_extend_fn, element_collision_fn,
restarts=50, iterations=100, smooth=smooth, max_time=max_time)
# path = plan_joint_motion(robot, joints, end_conf, obstacles=obstacles,
# self_collisions=SELF_COLLISIONS, disabled_collisions=disabled_collisions,
# weights=weights, resolutions=resolutions,
# restarts=50, iterations=100, smooth=100)
# if (bounding is not None) and (path is not None):
# for i, q in enumerate(path):
# print('{}/{}'.format(i, len(path)))
# set_joint_positions(robot, joints, q)
# wait_for_user()
if bounding is not None:
remove_body(bounding)
for hull in hulls:
remove_body(hull)
if path is None:
print('Failed to find a motion plan!')
return None
return MotionTrajectory(robot, joints, path)
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-c', '--cfree', action='store_true',
help='When enabled, disables collision checking.')
# parser.add_argument('-p', '--problem', default='test_pour', choices=sorted(problem_fn_from_name),
# help='The name of the problem to solve.')
parser.add_argument('--holonomic', action='store_true', # '-h',
help='')
parser.add_argument('-l', '--lock', action='store_false',
help='')
parser.add_argument('-s', '--seed', default=None, type=int,
help='The random seed to use.')
parser.add_argument('-v', '--viewer', action='store_false',
help='')
args = parser.parse_args()
connect(use_gui=args.viewer)
seed = args.seed
#seed = 0
#seed = time.time()
set_random_seed(seed=seed) # None: 2147483648 = 2**31
set_numpy_seed(seed=seed)
print('Random seed:', get_random_seed(), random.random())
print('Numpy seed:', get_numpy_seed(), np.random.random())
#########################
robot, base_limits, goal_conf, obstacles = problem1()
draw_base_limits(base_limits)
custom_limits = create_custom_base_limits(robot, base_limits)
base_joints = joints_from_names(robot, BASE_JOINTS)
base_link = link_from_name(robot, BASE_LINK_NAME)
if args.cfree:
obstacles = []
# for obstacle in obstacles:
# draw_aabb(get_aabb(obstacle)) # Updates automatically
resolutions = None
#resolutions = np.array([0.05, 0.05, math.radians(10)])
set_all_static() # Doesn't seem to affect
region_aabb = AABB(lower=-np.ones(3), upper=+np.ones(3))
draw_aabb(region_aabb)
step_simulation() # Need to call before get_bodies_in_region
#update_scene() # TODO: https://github.com/bulletphysics/bullet3/pull/3331
bodies = get_bodies_in_region(region_aabb)
print(len(bodies), bodies)
# https://github.com/bulletphysics/bullet3/search?q=broadphase
# https://github.com/bulletphysics/bullet3/search?p=1&q=getCachedOverlappingObjects&type=&utf8=%E2%9C%93
# https://andysomogyi.github.io/mechanica/bullet.html
# http://www.cs.kent.edu/~ruttan/GameEngines/lectures/Bullet_User_Manual
#draw_pose(get_link_pose(robot, base_link), length=0.5)
for conf in [get_joint_positions(robot, base_joints), goal_conf]:
draw_pose(pose_from_pose2d(conf, z=DRAW_Z), length=DRAW_LENGTH)
aabb = get_aabb(robot)
#aabb = get_subtree_aabb(robot, base_link)
draw_aabb(aabb)
for link in [BASE_LINK, base_link]:
print(link, get_collision_data(robot, link), pairwise_link_collision(robot, link, robot, link))
#########################
saver = WorldSaver()
start_time = time.time()
profiler = Profiler(field='tottime', num=50) # tottime | cumtime | None
profiler.save()
with LockRenderer(lock=args.lock):
path = plan_motion(robot, base_joints, goal_conf, holonomic=args.holonomic, obstacles=obstacles,
custom_limits=custom_limits, resolutions=resolutions,
use_aabb=True, cache=True, max_distance=0.,
restarts=2, iterations=20, smooth=20) # 20 | None
saver.restore()
#wait_for_duration(duration=1e-3)
profiler.restore()
#########################
solved = path is not None
length = INF if path is None else len(path)
cost = sum(get_distance_fn(robot, base_joints, weights=resolutions)(*pair) for pair in get_pairs(path))
print('Solved: {} | Length: {} | Cost: {:.3f} | Runtime: {:.3f} sec'.format(
solved, length, cost, elapsed_time(start_time)))
if path is None:
disconnect()
return
iterate_path(robot, base_joints, path)
disconnect()
def compute_cost(robot, joints, path, resolutions=None):
if path is None:
return INF
distance_fn = get_distance_fn(robot, joints,
weights=resolutions) # TODO: get_duration_fn
return sum(distance_fn(*pair) for pair in get_pairs(path))
def plan_approach(world,
approach_pose,
attachments=[],
obstacles=set(),
teleport=False,
switches_only=False,
approach_path=not MOVE_ARM,
**kwargs):
# TODO: use velocities in the distance function
distance_fn = get_distance_fn(world.robot, world.arm_joints)
aq = world.carry_conf
grasp_conf = get_joint_positions(world.robot, world.arm_joints)
if switches_only:
return [aq.values, grasp_conf]
# TODO: could extract out collision function
# TODO: track the full approach motion
full_approach_conf = world.solve_inverse_kinematics(
approach_pose, nearby_tolerance=NEARBY_APPROACH)
if full_approach_conf is None: # TODO: | {obj}
if PRINT_FAILURES: print('Pregrasp kinematic failure')
return None
moving_links = get_moving_links(world.robot, world.arm_joints)
robot_obstacle = (world.robot, frozenset(moving_links))
#robot_obstacle = world.robot
if any(pairwise_collision(robot_obstacle, b)
for b in obstacles): # TODO: | {obj}
if PRINT_FAILURES: print('Pregrasp collision failure')
return None
approach_conf = get_joint_positions(world.robot, world.arm_joints)
if teleport:
return [aq.values, approach_conf, grasp_conf]
distance = distance_fn(grasp_conf, approach_conf)
if MAX_CONF_DISTANCE < distance:
if PRINT_FAILURES:
print('Pregrasp proximity failure (distance={:.5f})'.format(
distance))
return None
resolutions = ARM_RESOLUTION * np.ones(len(world.arm_joints))
grasp_path = plan_direct_joint_motion(
world.robot,
world.arm_joints,
grasp_conf,
attachments=attachments,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS,
disabled_collisions=world.disabled_collisions,
custom_limits=world.custom_limits,
resolutions=resolutions / 4.)
if grasp_path is None:
if PRINT_FAILURES: print('Pregrasp path failure')
return None
if not approach_path:
return grasp_path
# TODO: plan one with attachment placed and one held
# TODO: can still use this as a witness that the conf is reachable
aq.assign()
approach_path = plan_joint_motion(
world.robot,
world.arm_joints,
approach_conf,
attachments=attachments,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS,
disabled_collisions=world.disabled_collisions,
custom_limits=world.custom_limits,
resolutions=resolutions,
restarts=2,
iterations=25,
smooth=25)
if approach_path is None:
if PRINT_FAILURES: print('Approach path failure')
return None
return approach_path + grasp_path
def get_cspace_distance(robot, q1, q2):
#return get_distance(q1, q2)
joints = get_movable_joints(robot)
distance_fn = get_distance_fn(robot, joints, weights=JOINT_WEIGHTS)
return distance_fn(q1, q2)