def test_close_gripper(robot, arm):
gripper_joints = get_gripper_joints(robot, arm)
extend_fn = get_extend_fn(robot, gripper_joints)
for positions in extend_fn(get_open_positions(robot, arm), get_closed_positions(robot, arm)):
set_joint_positions(robot, gripper_joints, positions)
print(positions)
wait_if_gui('Continue?')
def compute_door_paths(world,
joint_name,
door_conf1,
door_conf2,
obstacles=set(),
teleport=False):
door_paths = []
if door_conf1 == door_conf2:
return door_paths
door_joint = joint_from_name(world.kitchen, joint_name)
door_joints = [door_joint]
# TODO: could unify with grasp path
door_extend_fn = get_extend_fn(world.kitchen,
door_joints,
resolutions=[DOOR_RESOLUTION])
door_path = [door_conf1.values] + list(
door_extend_fn(door_conf1.values, door_conf2.values))
if teleport:
door_path = [door_conf1.values, door_conf2.values]
# TODO: open until collision for the drawers
sign = world.get_door_sign(door_joint)
pull = (sign * door_path[0][0] < sign * door_path[-1][0])
# door_obstacles = get_descendant_obstacles(world.kitchen, door_joint)
for handle_grasp in get_handle_grasps(world, door_joint, pull=pull):
link, grasp, pregrasp = handle_grasp
handle_path = []
for door_conf in door_path:
set_joint_positions(world.kitchen, door_joints, door_conf)
# if any(pairwise_collision(door_obst, obst)
# for door_obst, obst in product(door_obstacles, obstacles)):
# return
handle_path.append(get_link_pose(world.kitchen, link))
# Collide due to adjacency
# TODO: check pregrasp path as well
# TODO: check gripper self-collisions with the robot
set_configuration(world.gripper, world.open_gq.values)
tool_path = [
multiply(handle_pose, invert(grasp)) for handle_pose in handle_path
]
for i, tool_pose in enumerate(tool_path):
set_joint_positions(world.kitchen, door_joints, door_path[i])
set_tool_pose(world, tool_pose)
# handles = draw_pose(handle_path[i], length=0.25)
# handles.extend(draw_aabb(get_aabb(world.kitchen, link=link)))
# wait_for_user()
# for handle in handles:
# remove_debug(handle)
if any(
pairwise_collision(world.gripper, obst)
for obst in obstacles):
break
else:
door_paths.append(
DoorPath(door_path, handle_path, handle_grasp, tool_path))
return door_paths
def iterate(self, world, attachments):
joints = get_gripper_joints(world.robot, self.arm)
start_conf = get_joint_positions(world.robot, joints)
end_conf = [self.position] * len(joints)
extend_fn = get_extend_fn(world.robot, joints)
path = [start_conf] + list(extend_fn(start_conf, end_conf))
for positions in path:
set_joint_positions(world.robot, joints, positions)
yield positions
def plan_approach(end_effector,
print_traj,
collision_fn,
approach_distance=APPROACH_DISTANCE):
# TODO: collisions at the ends of elements
if approach_distance == 0:
return Command([print_traj])
robot = end_effector.robot
joints = get_movable_joints(robot)
extend_fn = get_extend_fn(robot,
joints,
resolutions=0.25 * JOINT_RESOLUTIONS)
tool_link = link_from_name(robot, TOOL_LINK)
approach_pose = Pose(Point(z=-approach_distance))
#element = print_traj.element
#midpoint = get_point(element)
#points = map(point_from_pose, [print_traj.tool_path[0], print_traj.tool_path[-1]])
#midpoint = np.average(list(points), axis=0)
#draw_point(midpoint)
#element_to_base = 0.05*get_unit_vector(get_point(robot) - midpoint)
#retreat_pose = Pose(Point(element_to_base))
# TODO: perpendicular to element
# TODO: solve_ik
start_conf = print_traj.path[0]
set_joint_positions(robot, joints, start_conf)
initial_pose = multiply(print_traj.tool_path[0], approach_pose)
#draw_pose(initial_pose)
#wait_if_gui()
initial_conf = inverse_kinematics(robot, tool_link, initial_pose)
if initial_conf is None:
return None
initial_path = [initial_conf] + list(extend_fn(initial_conf, start_conf))
if any(map(collision_fn, initial_path)):
return None
initial_traj = MotionTrajectory(robot, joints, initial_path)
end_conf = print_traj.path[-1]
set_joint_positions(robot, joints, end_conf)
final_pose = multiply(print_traj.tool_path[-1], approach_pose)
final_conf = inverse_kinematics(robot, tool_link, final_pose)
if final_conf is None:
return None
final_path = [end_conf] + list(extend_fn(
end_conf, final_conf)) # TODO: version that includes the endpoints
if any(map(collision_fn, final_path)):
return None
final_traj = MotionTrajectory(robot, joints, final_path)
return Command([initial_traj, print_traj, final_traj])
def fn(gq1, gq2):
#if gq1 == gq2:
# return None
if teleport:
path = [gq1.values, gq2.values]
else:
extend_fn = get_extend_fn(gq2.body,
gq2.joints,
resolutions=resolutions)
path = [gq1.values] + list(extend_fn(gq1.values, gq2.values))
cmd = Sequence(State(world),
commands=[
Trajectory(world, gq2.body, gq2.joints, path),
],
name='gripper')
return (cmd, )
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 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)