def lower_spoon(world, bowl_name, spoon_name, min_spoon_z, max_spoon_z):
assert min_spoon_z <= max_spoon_z
bowl_body = world.get_body(bowl_name)
bowl_urdf_from_center = get_urdf_from_base(
bowl_body) # get_urdf_from_center
spoon_body = world.get_body(spoon_name)
spoon_quat = lookup_orientation(spoon_name, STIR_QUATS)
spoon_urdf_from_center = get_urdf_from_base(
spoon_body, reference_quat=spoon_quat) # get_urdf_from_center
# Keeping the orientation consistent for generalization purposes
# TODO: what happens when the base isn't flat (e.g. bowl)
bowl_pose = get_pose(bowl_body)
stir_z = None
for z in np.arange(max_spoon_z, min_spoon_z, step=-0.005):
bowl_from_stirrer = multiply(bowl_urdf_from_center, Pose(Point(z=z)),
invert(spoon_urdf_from_center))
set_pose(spoon_body, multiply(bowl_pose, bowl_from_stirrer))
#wait_for_user()
if pairwise_collision(bowl_body, spoon_body):
# Want to be careful not to make contact with the base
break
stir_z = z
return stir_z
def sample_push_contact(world, feature, parameter, under=False):
robot = world.robot
arm = feature['arm_name']
body = world.get_body(feature['block_name'])
push_yaw = feature['push_yaw']
center, (width, _, height) = approximate_as_prism(body, body_pose=Pose(euler=Euler(yaw=push_yaw)))
max_backoff = width + 0.1 # TODO: add gripper bounding box
tool_link = link_from_name(robot, TOOL_FRAMES[arm])
tool_pose = get_link_pose(robot, tool_link)
gripper_link = link_from_name(robot, GRIPPER_LINKS[arm])
collision_links = get_link_subtree(robot, gripper_link)
urdf_from_center = Pose(point=center)
reverse_z = Pose(euler=Euler(pitch=math.pi))
rotate_theta = Pose(euler=Euler(yaw=push_yaw))
#translate_z = Pose(point=Point(z=-feature['block_height']/2. + parameter['gripper_z'])) # Relative to base
translate_z = Pose(point=Point(z=parameter['gripper_z'])) # Relative to center
tilt_gripper = Pose(euler=Euler(pitch=parameter['gripper_tilt']))
grasps = []
for i in range(1 + under):
flip_gripper = Pose(euler=Euler(yaw=i * math.pi))
for x in np.arange(0, max_backoff, step=0.01):
translate_x = Pose(point=Point(x=-x))
grasp_pose = multiply(flip_gripper, tilt_gripper, translate_x, translate_z, rotate_theta,
reverse_z, invert(urdf_from_center))
set_pose(body, multiply(tool_pose, TOOL_POSE, grasp_pose))
if not link_pairs_collision(robot, collision_links, body, collision_buffer=0.):
grasps.append(grasp_pose)
break
return grasps
def create_inverse_reachability(robot, body, table, arm, grasp_type, max_attempts=500, num_samples=500):
tool_link = get_gripper_link(robot, arm)
robot_saver = BodySaver(robot)
gripper_from_base_list = []
grasps = GET_GRASPS[grasp_type](body)
start_time = time.time()
while len(gripper_from_base_list) < num_samples:
box_pose = sample_placement(body, table)
set_pose(body, box_pose)
grasp_pose = random.choice(grasps)
gripper_pose = multiply(box_pose, invert(grasp_pose))
for attempt in range(max_attempts):
robot_saver.restore()
base_conf = next(uniform_pose_generator(robot, gripper_pose)) #, reachable_range=(0., 1.)))
#set_base_values(robot, base_conf)
set_group_conf(robot, 'base', base_conf)
if pairwise_collision(robot, table):
continue
grasp_conf = pr2_inverse_kinematics(robot, arm, gripper_pose) #, nearby_conf=USE_CURRENT)
#conf = inverse_kinematics(robot, link, gripper_pose)
if (grasp_conf is None) or pairwise_collision(robot, table):
continue
gripper_from_base = multiply(invert(get_link_pose(robot, tool_link)), get_base_pose(robot))
#wait_if_gui()
gripper_from_base_list.append(gripper_from_base)
print('{} / {} | {} attempts | [{:.3f}]'.format(
len(gripper_from_base_list), num_samples, attempt, elapsed_time(start_time)))
wait_if_gui()
break
else:
print('Failed to find a kinematic solution after {} attempts'.format(max_attempts))
return save_inverse_reachability(robot, arm, grasp_type, tool_link, gripper_from_base_list)
def compute_surface_aabb(world, surface_name):
if surface_name in ENV_SURFACES: # TODO: clean this up
# TODO: the aabb for golf is off the table
surface_body = world.environment_bodies[surface_name]
return get_aabb(surface_body)
surface_body = world.kitchen
surface_name, shape_name, _ = surface_from_name(surface_name)
surface_link = link_from_name(surface_body, surface_name)
surface_pose = get_link_pose(surface_body, surface_link)
if shape_name == SURFACE_TOP:
surface_aabb = get_aabb(surface_body, surface_link)
elif shape_name == SURFACE_BOTTOM:
data = sorted(get_collision_data(surface_body, surface_link),
key=lambda d: point_from_pose(get_data_pose(d))[2])[0]
extent = np.array(get_data_extents(data))
aabb = AABB(-extent/2., +extent/2.)
vertices = apply_affine(multiply(surface_pose, get_data_pose(data)), get_aabb_vertices(aabb))
surface_aabb = aabb_from_points(vertices)
else:
[data] = filter(lambda d: d.filename != '',
get_collision_data(surface_body, surface_link))
meshes = read_obj(data.filename)
#colors = spaced_colors(len(meshes))
#set_color(surface_body, link=surface_link, color=np.zeros(4))
mesh = meshes[shape_name]
#for i, (name, mesh) in enumerate(meshes.items()):
mesh = tform_mesh(multiply(surface_pose, get_data_pose(data)), mesh=mesh)
surface_aabb = aabb_from_points(mesh.vertices)
#add_text(surface_name, position=surface_aabb[1])
#draw_mesh(mesh, color=colors[i])
#wait_for_user()
#draw_aabb(surface_aabb)
#wait_for_user()
return surface_aabb
def get_ik_generator(robot, tool_pose):
from .ikfast_kuka_kr6_r900 import get_ik
world_from_base = get_link_pose(robot, link_from_name(robot, BASE_FRAME))
#world_from_base == get_pose(robot)
base_from_tool = multiply(invert(world_from_base), tool_pose)
base_from_ik = multiply(base_from_tool, get_tool_from_ik(robot))
yield compute_inverse_kinematics(get_ik, base_from_ik)
def get_grasps(world, name, grasp_types=GRASP_TYPES, pre_distance=APPROACH_DISTANCE, **kwargs):
use_width = world.robot_name == FRANKA_CARTER
body = world.get_body(name)
#fraction = 0.25
obj_type = type_from_name(name)
body_pose = REFERENCE_POSE.get(obj_type, unit_pose())
center, extent = approximate_as_prism(body, body_pose)
for grasp_type in grasp_types:
if not implies(world.is_real(), is_valid_grasp_type(name, grasp_type)):
continue
#assert is_valid_grasp_type(name, grasp_type)
if grasp_type == TOP_GRASP:
grasp_length = 1.5 * FINGER_EXTENT[2] # fraction = 0.5
pre_direction = pre_distance * get_unit_vector([0, 0, 1])
post_direction = unit_point()
generator = get_top_grasps(body, under=True, tool_pose=TOOL_POSE, body_pose=body_pose,
grasp_length=grasp_length, max_width=np.inf, **kwargs)
elif grasp_type == SIDE_GRASP:
# Take max of height and something
grasp_length = 1.75 * FINGER_EXTENT[2] # No problem if pushing a little
x, z = pre_distance * get_unit_vector([3, -1])
pre_direction = [0, 0, x]
post_direction = [0, 0, z]
top_offset = extent[2] / 2 if obj_type in MID_SIDE_GRASPS else 1.0*FINGER_EXTENT[0]
# Under grasps are actually easier for this robot
# TODO: bug in under in that it grasps at the bottom
generator = get_side_grasps(body, under=False, tool_pose=TOOL_POSE, body_pose=body_pose,
grasp_length=grasp_length, top_offset=top_offset, max_width=np.inf, **kwargs)
# if world.robot_name == FRANKA_CARTER else unit_pose()
generator = (multiply(Pose(euler=Euler(yaw=yaw)), grasp)
for grasp in generator for yaw in [0, np.pi])
else:
raise ValueError(grasp_type)
grasp_poses = randomize(list(generator))
if obj_type in CYLINDERS:
# TODO: filter first
grasp_poses = (multiply(grasp_pose, Pose(euler=Euler(
yaw=random.uniform(-math.pi, math.pi)))) for grasp_pose in cycle(grasp_poses))
for i, grasp_pose in enumerate(grasp_poses):
pregrasp_pose = multiply(Pose(point=pre_direction), grasp_pose,
Pose(point=post_direction))
grasp = Grasp(world, name, grasp_type, i, grasp_pose, pregrasp_pose)
with BodySaver(body):
grasp.get_attachment().assign()
with BodySaver(world.robot):
grasp.grasp_width = close_until_collision(
world.robot, world.gripper_joints, bodies=[body])
#print(get_joint_positions(world.robot, world.arm_joints)[-1])
#draw_pose(unit_pose(), parent=world.robot, parent_link=world.tool_link)
#grasp.get_attachment().assign()
#wait_for_user()
##for value in get_joint_limits(world.robot, world.arm_joints[-1]):
#for value in [-1.8973, 0, +1.8973]:
# set_joint_position(world.robot, world.arm_joints[-1], value)
# grasp.get_attachment().assign()
# wait_for_user()
if use_width and (grasp.grasp_width is None):
continue
yield grasp
def test_print(robot, node_points, elements):
#elements = [elements[0]]
elements = [elements[-1]]
[element_body] = create_elements(node_points, elements)
wait_for_user()
phi = 0
#grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=phi, yaw=theta))
# for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)]
#grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=theta, yaw=phi))
# for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)]
grasp_rotations = [sample_direction() for _ in range(25)]
link = link_from_name(robot, TOOL_NAME)
movable_joints = get_movable_joints(robot)
sample_fn = get_sample_fn(robot, movable_joints)
for grasp_rotation in grasp_rotations:
n1, n2 = elements[0]
length = np.linalg.norm(node_points[n2] - node_points[n1])
set_joint_positions(robot, movable_joints, sample_fn())
for t in np.linspace(-length / 2, length / 2, 10):
#element_translation = Pose(point=Point(z=-t))
#grasp_pose = multiply(grasp_rotation, element_translation)
#reverse = Pose(euler=Euler())
reverse = Pose(euler=Euler(roll=np.pi))
grasp_pose = get_grasp_pose(t, grasp_rotation, 0)
grasp_pose = multiply(grasp_pose, reverse)
element_pose = get_pose(element_body)
link_pose = multiply(element_pose, invert(grasp_pose))
conf = inverse_kinematics(robot, link, link_pose)
wait_for_user()
def test_grasps(robot, node_points, elements):
element = elements[-1]
[element_body] = create_elements(node_points, [element])
phi = 0
link = link_from_name(robot, TOOL_NAME)
link_pose = get_link_pose(robot, link)
draw_pose(link_pose) #, parent=robot, parent_link=link)
wait_for_user()
n1, n2 = element
p1 = node_points[n1]
p2 = node_points[n2]
length = np.linalg.norm(p2 - p1)
# Bottom of cylinder is p1, top is p2
print(element, p1, p2)
for phi in np.linspace(0, np.pi, 10, endpoint=True):
theta = np.pi / 4
for t in np.linspace(-length / 2, length / 2, 10):
translation = Pose(
point=Point(z=-t)
) # Want to be moving backwards because +z is out end effector
direction = Pose(euler=Euler(roll=np.pi / 2 + theta, pitch=phi))
angle = Pose(euler=Euler(yaw=1.2))
grasp_pose = multiply(angle, direction, translation)
element_pose = multiply(link_pose, grasp_pose)
set_pose(element_body, element_pose)
wait_for_user()
def main():
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
connect(use_gui=True)
with LockRenderer():
draw_pose(unit_pose(), length=1, width=1)
floor = create_floor()
set_point(floor, Point(z=-EPSILON))
table1 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2,
top_color=TAN, cylinder=False)
set_point(table1, Point(y=+0.5))
table2 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2,
top_color=TAN, cylinder=False)
set_point(table2, Point(y=-0.5))
tables = [table1, table2]
#set_euler(table1, Euler(yaw=np.pi/2))
with HideOutput():
# data_path = add_data_path()
# robot_path = os.path.join(data_path, WSG_GRIPPER)
robot_path = get_model_path(WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF
robot = load_pybullet(robot_path, fixed_base=True)
#dump_body(robot)
block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED)
block_z = stable_z(block1, table1)
set_point(block1, Point(y=-0.5, z=block_z))
block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN)
set_point(block2, Point(x=-0.25, y=-0.5, z=block_z))
block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE)
set_point(block3, Point(x=-0.15, y=+0.5, z=block_z))
blocks = [block1, block2, block3]
set_camera_pose(camera_point=Point(x=-1, z=block_z+1), target_point=Point(z=block_z))
block_pose = get_pose(block1)
open_gripper(robot)
tool_link = link_from_name(robot, 'tool_link')
base_from_tool = get_relative_pose(robot, tool_link)
#draw_pose(unit_pose(), parent=robot, parent_link=tool_link)
grasps = get_side_grasps(block1, tool_pose=Pose(euler=Euler(yaw=np.pi/2)),
top_offset=0.02, grasp_length=0.03, under=False)[1:2]
for grasp in grasps:
gripper_pose = multiply(block_pose, invert(grasp))
set_pose(robot, multiply(gripper_pose, invert(base_from_tool)))
wait_for_user()
wait_for_user('Finish?')
disconnect()
def iterate_approach_path(world, pose, grasp, body=None):
world_from_body = pose.get_world_from_body()
grasp_pose = multiply(world_from_body, invert(grasp.grasp_pose))
approach_pose = multiply(world_from_body, invert(grasp.pregrasp_pose))
for tool_pose in interpolate_poses(grasp_pose, approach_pose):
set_tool_pose(world, tool_pose)
if body is not None:
set_pose(body, multiply(tool_pose, grasp.grasp_pose))
yield
def gen_fn(arm, obj_name, pose):
open_width = get_max_limit(world.robot,
get_gripper_joints(world.robot, arm)[0])
obj_body = world.bodies[obj_name]
#grasps = cycle([(grasp,)])
grasps = cycle(grasp_gen_fn(obj_name))
for attempt in range(max_attempts):
try:
grasp, = next(grasps)
except StopIteration:
break
# TODO: if already successful for a grasp, continue
set_pose(obj_body, pose)
set_gripper_position(world.robot, arm, open_width)
robot_saver = BodySaver(
world.robot) # TODO: robot might be at the wrong conf
body_saver = BodySaver(obj_body)
pretool_pose = multiply(pose, invert(grasp.pregrasp_pose))
tool_pose = multiply(pose, invert(grasp.grasp_pose))
grip_conf = solve_inverse_kinematics(world.robot,
arm,
tool_pose,
obstacles=obstacles)
if grip_conf is None:
continue
#attachment = Attachment(world.robot, tool_link, get_grasp_pose(grasp), world.bodies[obj])
# Attachments cause table collisions
post_path = plan_waypoint_motion(
world.robot,
arm,
pretool_pose,
obstacles=obstacles,
attachments=[], #attachments=[attachment],
self_collisions=collisions,
disabled_collisions=disabled_collisions)
if post_path is None:
continue
pre_conf = Conf(post_path[-1])
pre_path = post_path[::-1]
post_conf = pre_conf
control = Control({
'action':
'pick',
'objects': [obj_name],
'context':
Context(savers=[robot_saver, body_saver], attachments={}),
'commands': [
ArmTrajectory(arm, pre_path, dialation=2.),
GripperCommand(arm, grasp.grasp_width,
effort=grasp.effort),
Attach(arm, obj_name, effort=grasp.effort),
ArmTrajectory(arm, post_path, dialation=2.),
],
})
yield (grasp, pre_conf, post_conf, control)
def get_tool_pose(robot, arm):
from .ikfast_eth_rfl import get_fk
ik_joints = get_torso_arm_joints(robot, arm)
conf = get_joint_positions(robot, ik_joints)
# TODO: this should be linked to ikfast's get numOfJoint junction
base_from_ik = compute_forward_kinematics(get_fk, conf)
base_from_tool = multiply(base_from_ik,
invert(get_tool_from_ik(robot, arm)))
world_from_base = get_link_pose(robot,
link_from_name(robot, IK_BASE_FRAMES[arm]))
return multiply(world_from_base, base_from_tool)
def get_water_path(bowl_body, bowl_pose, cup_body, pose_waypoints):
cup_pose = pose_waypoints[0]
bowl_origin_from_base = get_urdf_from_base(
bowl_body) # TODO: reference pose
cup_origin_from_base = get_urdf_from_base(cup_body)
ray_start = point_from_pose(multiply(cup_pose, cup_origin_from_base))
ray_end = point_from_pose(multiply(bowl_pose, bowl_origin_from_base))
water_path = interpolate_poses((ray_start, unit_quat()),
(ray_end, unit_quat()),
pos_step_size=0.01)
return water_path
def gen_fn(arm, button):
gripper_joint = get_gripper_joints(world.robot, arm)[0]
closed_width, open_width = get_joint_limits(world.robot, gripper_joint)
pose = world.initial_poses[button]
body = world.bodies[button]
presses = cycle(get_top_presses(body, tool_pose=TOOL_POSE))
for attempt in range(max_attempts):
try:
press = next(presses)
except StopIteration:
break
set_gripper_position(world.robot, arm, closed_width)
tool_pose = multiply(pose, invert(press))
grip_conf = solve_inverse_kinematics(
world.robot,
arm,
tool_pose,
obstacles=obstacle_bodies,
collision_buffer=collision_buffer)
if grip_conf is None:
continue
pretool_pose = multiply(tool_pose, Pose(point=pre_direction))
post_path = plan_waypoint_motion(
world.robot,
arm,
pretool_pose,
obstacles=obstacle_bodies,
collision_buffer=collision_buffer,
self_collisions=collisions,
disabled_collisions=disabled_collisions)
if post_path is None:
continue
post_conf = Conf(post_path[-1])
pre_conf = post_conf
pre_path = post_path[::-1]
control = Control({
'action':
'press',
'objects': [],
'context':
Context( # TODO: robot might be at the wrong conf
savers=[BodySaver(world.robot)
], # TODO: start with open instead
attachments={}),
'commands': [
GripperCommand(arm, closed_width),
ArmTrajectory(arm, pre_path),
ArmTrajectory(arm, post_path),
GripperCommand(arm, open_width),
],
})
yield (pre_conf, post_conf, control)
def get_ik_generator(robot, arm, tool_pose, **kwargs):
from .ikfast_eth_rfl import get_ik
world_from_base = get_link_pose(robot,
link_from_name(robot, IK_BASE_FRAMES[arm]))
base_from_tool = multiply(invert(world_from_base), tool_pose)
base_from_ik = multiply(base_from_tool, get_tool_from_ik(robot, arm))
sampled_limits = get_ik_limits(robot, get_torso_joint(robot, arm),
**kwargs)
while True:
sampled_values = [random.uniform(*sampled_limits)]
ik_joints = get_torso_arm_joints(robot, arm)
confs = compute_inverse_kinematics(get_ik, base_from_ik,
sampled_values)
yield [q for q in confs if not violates_limits(robot, ik_joints, q)]
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 gen_fn(index, pose, grasp):
body = brick_from_index[index].body
set_pose(body, pose.value)
obstacles = list(obstacle_from_name.values()) # + [body]
collision_fn = get_collision_fn(
robot,
movable_joints,
obstacles=obstacles,
attachments=[],
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions,
custom_limits=get_custom_limits(robot))
attach_pose = multiply(pose.value, invert(grasp.attach))
approach_pose = multiply(attach_pose, (approach_vector, unit_quat()))
# approach_pose = multiply(pose.value, invert(grasp.approach))
for _ in range(max_attempts):
if USE_IKFAST:
attach_conf = sample_tool_ik(robot, attach_pose)
else:
set_joint_positions(robot, movable_joints,
sample_fn()) # Random seed
attach_conf = inverse_kinematics(robot, tool_link, attach_pose)
if (attach_conf is None) or collision_fn(attach_conf):
continue
set_joint_positions(robot, movable_joints, attach_conf)
#if USE_IKFAST:
# approach_conf = sample_tool_ik(robot, approach_pose, nearby_conf=attach_conf)
#else:
approach_conf = inverse_kinematics(robot, tool_link, approach_pose)
if (approach_conf is None) or collision_fn(approach_conf):
continue
set_joint_positions(robot, movable_joints, approach_conf)
path = plan_direct_joint_motion(
robot,
movable_joints,
attach_conf,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions)
if path is None: # TODO: retreat
continue
#path = [approach_conf, attach_conf]
attachment = Attachment(robot, tool_link, grasp.attach, body)
traj = MotionTrajectory(robot,
movable_joints,
path,
attachments=[attachment])
yield approach_conf, traj
break
def sample_tool_ik(robot, arm, world_from_target, max_attempts=10, **kwargs):
world_from_tool = get_link_pose(
robot, link_from_name(robot, PR2_TOOL_FRAMES[arm]))
world_from_ik = get_link_pose(robot, link_from_name(robot, IK_LINK[arm]))
tool_from_ik = multiply(invert(world_from_tool), world_from_ik)
ik_pose = multiply(world_from_target, tool_from_ik)
generator = get_ik_generator(robot, arm, ik_pose, **kwargs)
for _ in range(max_attempts):
try:
solutions = next(generator)
if solutions:
return random.choice(solutions)
except StopIteration:
break
return None
def create_inverse_reachability2(robot, body, table, arm, grasp_type, max_attempts=500, num_samples=500):
tool_link = get_gripper_link(robot, arm)
problem = MockProblem(robot, fixed=[table], grasp_types=[grasp_type])
placement_gen_fn = get_stable_gen(problem)
grasp_gen_fn = get_grasp_gen(problem, collisions=True)
ik_ir_fn = get_ik_ir_gen(problem, max_attempts=max_attempts, learned=False, teleport=True)
placement_gen = placement_gen_fn(body, table)
grasps = list(grasp_gen_fn(body))
print('Grasps:', len(grasps))
# TODO: sample the torso height
# TODO: consider IK with respect to the torso frame
start_time = time.time()
gripper_from_base_list = []
while len(gripper_from_base_list) < num_samples:
[(p,)] = next(placement_gen)
(g,) = random.choice(grasps)
output = next(ik_ir_fn(arm, body, p, g), None)
if output is None:
print('Failed to find a solution after {} attempts'.format(max_attempts))
else:
(_, ac) = output
[at,] = ac.commands
at.path[-1].assign()
gripper_from_base = multiply(invert(get_link_pose(robot, tool_link)), get_base_pose(robot))
gripper_from_base_list.append(gripper_from_base)
print('{} / {} [{:.3f}]'.format(
len(gripper_from_base_list), num_samples, elapsed_time(start_time)))
wait_if_gui()
return save_inverse_reachability(robot, arm, grasp_type, tool_link, gripper_from_base_list)
def get_ik_generator(robot, tool_pose, track_limits=False, prev_free_list=[]):
from .ikfast_abb_irb6600_track import get_ik
world_from_base = get_link_pose(robot, link_from_name(robot, BASE_FRAME))
base_from_tool = multiply(invert(world_from_base), tool_pose)
base_from_ik = multiply(base_from_tool, get_tool_from_ik(robot))
sampled_limits = get_ik_limits(robot, joint_from_name(robot, *TRACK_JOINT),
track_limits)
while True:
if not prev_free_list:
sampled_values = [random.uniform(*sampled_limits)]
else:
sampled_values = prev_free_list
ik_joints = get_track_arm_joints(robot)
confs = compute_inverse_kinematics(get_ik, base_from_ik,
sampled_values)
yield [q for q in confs if not violates_limits(robot, ik_joints, q)]
def observe_pybullet(world):
# TODO: randomize robot's pose
# TODO: probabilities based on whether in viewcone or not
# TODO: sample from poses on table
# world_saver = WorldSaver()
visible_entities = are_visible(world)
detections = {}
assert OBS_P_FP == 0
for name in visible_entities:
# TODO: false positives
if random.random() < OBS_P_FN:
continue
body = world.get_body(name)
pose = get_pose(body)
dx, dy = np.random.multivariate_normal(mean=np.zeros(2),
cov=math.pow(OBS_POS_STD, 2) *
np.eye(2))
dyaw, = np.random.multivariate_normal(mean=np.zeros(1),
cov=math.pow(OBS_ORI_STD, 2) *
np.eye(1))
print('{}: dx={:.3f}, dy={:.3f}, dyaw={:.5f}'.format(
name, dx, dy, dyaw))
noise_pose = Pose(Point(x=dx, y=dy), Euler(yaw=dyaw))
observed_pose = multiply(pose, noise_pose)
#world.get_body_type(name)
detections.setdefault(name, []).append(
observed_pose) # TODO: use type instead
#world_saver.restore()
return detections
def compute_tool_path(element_pose, translation_path, direction, angle,
reverse):
tool_path = []
for translation in translation_path:
grasp_pose = get_grasp_pose(translation, direction, angle, reverse)
tool_path.append(multiply(element_pose, invert(grasp_pose)))
return tool_path
def test_retraction(robot, info, tool_link, distance=0.1, **kwargs):
ik_joints = get_ik_joints(robot, info, tool_link)
start_pose = get_link_pose(robot, tool_link)
end_pose = multiply(start_pose, Pose(Point(z=-distance)))
handles = [
add_line(point_from_pose(start_pose),
point_from_pose(end_pose),
color=BLUE)
]
#handles.extend(draw_pose(start_pose))
#handles.extend(draw_pose(end_pose))
path = []
pose_path = list(
interpolate_poses(start_pose, end_pose, pos_step_size=0.01))
for i, pose in enumerate(pose_path):
print('Waypoint: {}/{}'.format(i + 1, len(pose_path)))
handles.extend(draw_pose(pose))
conf = next(
either_inverse_kinematics(robot, info, tool_link, pose, **kwargs),
None)
if conf is None:
print('Failure!')
path = None
wait_for_user()
break
set_joint_positions(robot, ik_joints, conf)
path.append(conf)
wait_for_user()
# for conf in islice(ikfast_inverse_kinematics(robot, info, tool_link, pose, max_attempts=INF, max_distance=0.5), 1):
# set_joint_positions(robot, joints[:len(conf)], conf)
# wait_for_user()
remove_handles(handles)
return path
def get_handle_grasps(world, joint, pull=True, pre_distance=APPROACH_DISTANCE):
pre_direction = pre_distance * get_unit_vector([0, 0, 1])
#half_extent = 1.0*FINGER_EXTENT[2] # Collides
half_extent = 1.05 * FINGER_EXTENT[2]
grasps = []
for link in get_link_subtree(world.kitchen, joint):
if 'handle' in get_link_name(world.kitchen, link):
# TODO: can adjust the position and orientation on the handle
for yaw in [0, np.pi]: # yaw=0 DOESN'T WORK WITH LULA
handle_grasp = (Point(z=-half_extent),
quat_from_euler(
Euler(roll=np.pi, pitch=np.pi / 2,
yaw=yaw)))
#if not pull:
# handle_pose = get_link_pose(world.kitchen, link)
# for distance in np.arange(0., 0.05, step=0.001):
# pregrasp = multiply(([0, 0, -distance], unit_quat()), handle_grasp)
# tool_pose = multiply(handle_pose, invert(pregrasp))
# set_tool_pose(world, tool_pose)
# # TODO: check collisions
# wait_for_user()
handle_pregrasp = multiply((pre_direction, unit_quat()),
handle_grasp)
grasps.append(HandleGrasp(link, handle_grasp, handle_pregrasp))
return grasps
def plan_attachment_motion(robot,
arm,
attachment,
attachment_path,
obstacles,
collision_buffer,
attachment_collisions=True,
**kwargs):
attachment_body = attachment.child
if attachment_collisions and cartesian_path_collision(
attachment_body,
attachment_path,
obstacles,
collision_buffer=collision_buffer):
return None
tool_path = [
multiply(p, invert(attachment.grasp_pose)) for p in attachment_path
]
grip_conf = solve_inverse_kinematics(robot,
arm,
tool_path[0],
obstacles=obstacles)
if grip_conf is None:
return None
attachments = [attachment] if attachment_collisions else []
return plan_workspace_motion(robot,
arm,
tool_path,
attachments=attachments,
obstacles=obstacles,
collision_buffer=collision_buffer,
**kwargs)
def add_scales(task, ros_world):
scale_stackings = {}
holding = {grasp.obj_name for grasp in task.init_holding.values()}
with ClientSaver(ros_world.client):
perception = ros_world.perception
items = sorted(set(perception.get_items()) - holding,
key=lambda n: get_point(ros_world.get_body(n))[1],
reverse=False) # Right to left
for i, item in enumerate(items):
if not POUR and (get_type(item) != SCOOP_CUP):
continue
item_body = ros_world.get_body(item)
scale = create_name(SCALE_TYPE, i + 1)
with HideOutput():
scale_body = load_pybullet(get_body_urdf(get_type(scale)),
fixed_base=True)
ros_world.perception.sim_bodies[scale] = scale_body
ros_world.perception.sim_items[scale] = None
item_z = stable_z(item_body, scale_body)
scale_pose_item = Pose(
point=Point(z=-item_z)) # TODO: relies on origin in base
set_pose(scale_body, multiply(get_pose(item_body),
scale_pose_item))
roll, pitch, _ = get_euler(scale_body)
set_euler(scale_body, [roll, pitch, 0])
scale_stackings[scale] = item
#wait_for_user()
return scale_stackings
def sample_stir_trajectory(world, feature, parameter):
bowl_urdf_from_center = get_urdf_from_base(
world.bodies[feature['bowl_name']])
stirrer_quat = lookup_orientation(feature['spoon_name'], STIR_QUATS)
stirrer_urdf_from_center = get_urdf_from_base(
world.bodies[feature['spoon_name']], reference_quat=stirrer_quat)
stir_pos = np.array([0., 0., parameter['stir_z']])
entry_pos = np.array([0, 0, parameter['entry_z']])
# TODO: could rotate the wrist in place
# TODO: could reverse the trajectory afterwards
# TODO: could connect up the start and end
step_size = np.pi / 16
stir_positions = [
stir_pos + parameter['stir_radius'] *
np.array([math.cos(theta), math.sin(theta), 0])
for theta in np.arange(0, parameter['revolutions'] * 2 *
np.pi, step_size)
] # Counter-clockwise
entry_pos = stir_positions[0] + (entry_pos - stir_pos)
exit_pos = stir_positions[-1] + (entry_pos - stir_pos)
initial_yaw = random.uniform(-np.pi, np.pi)
rotate_stirrer = Pose(euler=Euler(yaw=initial_yaw))
# TODO(caelan): check the ordering/inversion when references are not the identity
return [
multiply(bowl_urdf_from_center, Pose(point=point), rotate_stirrer,
invert(stirrer_urdf_from_center))
for point in ([entry_pos] + stir_positions + [exit_pos])
]
def relative_detections(belief, detections):
world = belief.world
rel_detections = {}
world_aabb = world.get_world_aabb()
for name in detections:
if name == belief.holding:
continue
body = world.get_body(name)
for observed_pose in detections[name]:
world_z_axis = np.array([0, 0, 1])
local_z_axis = tform_point(observed_pose, world_z_axis)
if np.pi / 2 < angle_between(world_z_axis, local_z_axis):
observed_pose = multiply(observed_pose,
Pose(euler=Euler(roll=np.pi)))
if not aabb_contains_point(point_from_pose(observed_pose),
world_aabb):
continue
set_pose(body, observed_pose)
support = world.get_supporting(name)
#assert support is not None
# Could also fix as relative to the world
if support is None:
# TODO: prune if nowhere near a surface (e.g. on the robot)
relative_pose = create_world_pose(world, name, init=True)
else:
relative_pose = create_relative_pose(world,
name,
support,
init=True)
rel_detections.setdefault(name, []).append(relative_pose)
# relative_pose.assign()
return rel_detections
def gen_fn(index):
brick = brick_from_index[index]
while True:
original_grasp = random.choice(brick.grasps)
theta = random.uniform(-np.pi, +np.pi)
rotation = Pose(euler=Euler(yaw=theta))
new_attach = multiply(rotation, original_grasp.attach)
grasp = Grasp(None, None, None, new_attach, None)
yield grasp,
def get_tool_pose(robot):
from .ikfast_kuka_kr6_r900 import get_fk
ik_joints = get_movable_joints(robot)
conf = get_joint_positions(robot, ik_joints)
# TODO: this should be linked to ikfast's get numOfJoint function
assert len(conf) == 6
base_from_tool = compute_forward_kinematics(get_fk, conf)
world_from_base = get_link_pose(robot, link_from_name(robot, BASE_FRAME))
return multiply(world_from_base, base_from_tool)