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 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 pour_path_from_parameter(world, bowl_name, cup_name):
bowl_body = world.get_body(bowl_name)
bowl_center, (bowl_d, bowl_h) = approximate_as_cylinder(bowl_body)
cup_body = world.get_body(cup_name)
cup_center, (cup_d, _, cup_h) = approximate_as_prism(cup_body)
#####
obj_type = type_from_name(cup_name)
if obj_type in [MUSTARD]:
initial_pitch = final_pitch = -np.pi
radius = 0
else:
initial_pitch = 0 # different if mustard
final_pitch = -3 * np.pi / 4
radius = bowl_d / 2
#axis_in_cup_center_x = -0.05
axis_in_cup_center_x = 0 # meters
#axis_in_cup_center_z = -cup_h/2.
axis_in_cup_center_z = 0. # meters
#axis_in_cup_center_z = +cup_h/2.
# tl := top left | tr := top right
cup_tl_in_center = np.array([-cup_d / 2, 0, cup_h / 2])
cup_tl_in_axis = cup_tl_in_center - Point(z=axis_in_cup_center_z)
cup_tl_angle = np.math.atan2(cup_tl_in_axis[2], cup_tl_in_axis[0])
cup_tl_pour_pitch = final_pitch - cup_tl_angle
cup_radius2d = np.linalg.norm([cup_tl_in_axis])
pivot_in_bowl_tr = Point(
x=-(cup_radius2d * np.math.cos(cup_tl_pour_pitch) + 0.01),
z=(cup_radius2d * np.math.sin(cup_tl_pour_pitch) + Z_OFFSET))
pivot_in_bowl_center = Point(x=radius, z=bowl_h / 2) + pivot_in_bowl_tr
base_from_pivot = Pose(
Point(x=axis_in_cup_center_x, z=axis_in_cup_center_z))
#####
assert -np.pi <= final_pitch <= initial_pitch
pitches = [initial_pitch]
if final_pitch != initial_pitch:
pitches = list(np.arange(final_pitch, initial_pitch,
np.pi / 16)) + pitches
cup_path_in_bowl = []
for pitch in pitches:
rotate_pivot = Pose(
euler=Euler(pitch=pitch)
) # Can also interpolate directly between start and end quat
cup_path_in_bowl.append(
multiply(Pose(point=bowl_center), Pose(pivot_in_bowl_center),
rotate_pivot, invert(base_from_pivot),
invert(Pose(point=cup_center))))
return cup_path_in_bowl
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 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 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 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 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 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 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 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 rotate_assembly(elements, node_points, ground_nodes, yaw=0.):
# TODO: more general affine transformations
world_from_base = Pose(point=get_base_centroid(node_points, ground_nodes))
points_base = apply_affine(invert(world_from_base), node_points)
rotation = Pose(euler=Euler(yaw=yaw))
points_world = list(map(np.array, apply_affine(multiply(world_from_base, rotation), points_base)))
#draw_model(elements, scaled_node_points, ground_nodes, color=RED)
#wait_if_gui()
return points_world
def gen_fn(arm, bowl_name, bowl_pose, stirrer_name, grasp):
if bowl_name == stirrer_name:
return
bowl_body = world.bodies[bowl_name]
attachment = get_grasp_attachment(world, arm, grasp)
feature = get_stir_feature(world, bowl_name, stirrer_name)
parameter_generator = parameter_fn(world, feature)
if revisit:
parameter_generator = cycle(parameter_generator)
for parameter in parameter_generator:
for _ in range(max_attempts):
set_gripper_position(world.robot, arm, grasp.grasp_width)
set_pose(bowl_body, bowl_pose)
stirrer_path_bowl = sample_stir_trajectory(
world, feature, parameter)
rotate_bowl = Pose(euler=Euler(
yaw=random.uniform(-np.pi, np.pi)))
stirrer_path = [
multiply(bowl_pose, invert(rotate_bowl), cup_pose_bowl)
for cup_pose_bowl in stirrer_path_bowl
]
#visualize_cartesian_path(world.get_body(stirrer_name), stirrer_path)
arm_path = plan_attachment_motion(
world.robot,
arm,
attachment,
stirrer_path,
obstacles=set(obstacles) - {bowl_body},
self_collisions=collisions,
disabled_collisions=disabled_collisions,
collision_buffer=collision_buffer,
attachment_collisions=False)
if arm_path is None:
continue
pre_conf = Conf(arm_path[0])
post_conf = Conf(arm_path[-1])
control = Control({
'action':
'stir',
'objects': [bowl_name, stirrer_name],
'feature':
feature,
'parameter':
parameter,
'context':
Context(
savers=[BodySaver(world.robot)
], # TODO: robot might be at the wrong conf
attachments={stirrer_name: attachment}),
'commands': [
ArmTrajectory(arm, arm_path),
],
})
yield (pre_conf, post_conf, control)
def pour_path_from_parameter(world, feature, parameter, cup_yaw=None):
cup_body = world.get_body(feature['cup_name'])
#cup_urdf_from_center = get_urdf_from_center(cup_body, reference_quat=get_liquid_quat(feature['cup_name']))
ref_from_urdf = (unit_point(), get_liquid_quat(feature['cup_name']))
cup_center_in_ref, _ = approximate_as_prism(cup_body,
body_pose=ref_from_urdf)
cup_center_in_ref[:
2] = 0 # Assumes the xy pour center is specified by the URDF (e.g. for spoons)
cup_urdf_from_center = multiply(invert(ref_from_urdf),
Pose(point=cup_center_in_ref))
# TODO: allow some deviation around cup_yaw for spoons
if cup_yaw is None:
cup_yaw = random.choice([0, np.pi]) if 'spoon' in feature['cup_name'] \
else random.uniform(-np.pi, np.pi)
z_rotate_cup = Pose(euler=Euler(yaw=cup_yaw))
bowl_from_pivot = get_bowl_from_pivot(world, feature, parameter)
if RELATIVE_POUR:
parameter = scale_parameter(feature,
parameter,
RELATIVE_POUR_SCALING,
descale=True)
base_from_pivot = Pose(
Point(x=parameter['axis_in_cup_x'], z=parameter['axis_in_cup_z']))
initial_pitch = 0
final_pitch = parameter['pitch']
assert -np.pi <= final_pitch <= initial_pitch
cup_path_in_bowl = []
for pitch in list(np.arange(final_pitch, initial_pitch,
np.pi / 16)) + [initial_pitch]:
rotate_pivot = Pose(
euler=Euler(pitch=pitch)
) # Can also interpolate directly between start and end quat
cup_path_in_bowl.append(
multiply(bowl_from_pivot, rotate_pivot, invert(base_from_pivot),
z_rotate_cup, invert(cup_urdf_from_center)))
cup_times = constant_velocity_times(cup_path_in_bowl)
# TODO: check for collisions here?
return cup_path_in_bowl, cup_times
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 _create_robot(self):
with ClientSaver(self.client):
with HideOutput():
pr2_path = os.path.join(get_models_path(), PR2_URDF)
self.pr2 = load_pybullet(pr2_path, fixed_base=True)
# Base link is the origin
base_pose = get_link_pose(self.robot,
link_from_name(self.robot, BASE_FRAME))
set_pose(self.robot, invert(base_pose))
return self.pr2
def get_urdf_from_z_axis(body, z_fraction, reference_quat=unit_quat()):
# AKA the pose of the body's center wrt to the body's origin
# z_fraction=0. => bottom, z_fraction=0.5 => center, z_fraction=1. => top
ref_from_urdf = (unit_point(), reference_quat)
center_in_ref, (_,
height) = approximate_as_cylinder(body,
body_pose=ref_from_urdf)
center_in_ref[2] += (z_fraction - 0.5) * height
ref_from_center = (center_in_ref, unit_quat()
) # Maps from center frame to origin frame
urdf_from_center = multiply(invert(ref_from_urdf), ref_from_center)
return urdf_from_center
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 gen(obj_name, pose, grasp, *args):
obstacles = world.static_obstacles | get_surface_obstacles(
world, pose.support)
#if not collisions:
# obstacles = set()
if not is_approach_safe(world, obj_name, pose, grasp, obstacles):
return
# TODO: check collisions with obj at pose
gripper_pose = multiply(
pose.get_world_from_body(),
invert(grasp.grasp_pose)) # w_f_g = w_f_o * (g_f_o)^-1
if learned:
base_generator = cycle(
load_place_base_poses(world, gripper_pose, pose.support,
grasp.grasp_type))
else:
base_generator = uniform_pose_generator(world.robot, gripper_pose)
safe_base_generator = inverse_reachability(world,
base_generator,
obstacles=obstacles,
**kwargs)
while True:
for i in range(max_attempts):
try:
base_conf = next(safe_base_generator)
except StopIteration:
return
if base_conf is None:
yield None
continue # TODO: could break if not pose.init
randomize = (random.random() < P_RANDOMIZE_IK)
ik_outputs = next(
plan_pick(world,
obj_name,
pose,
grasp,
base_conf,
obstacles,
randomize=randomize,
**kwargs), None)
if ik_outputs is not None:
print('Pick succeeded after {} attempts'.format(i))
yield (base_conf, ) + ik_outputs
break
else:
if PRINT_FAILURES:
print(
'Pick failure after {} attempts'.format(max_attempts))
#if not pose.init: # Might be an intended placement blocked by a drawer
# break
yield None
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 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 test(object_name, pose, base_conf):
if object_name in ALL_SURFACES:
surface_name = object_name
if surface_name not in vertices_from_surface:
vertices_from_surface[surface_name] = grow_polygon(
map(point_from_pose,
load_inverse_placements(world, surface_name)),
radius=GROW_INVERSE_BASE)
if not vertices_from_surface[surface_name]:
return False
base_conf.assign()
pose.assign()
surface = surface_from_name(surface_name)
world_from_surface = get_link_pose(
world.kitchen, link_from_name(world.kitchen, surface.link))
world_from_base = get_link_pose(world.robot, world.base_link)
surface_from_base = multiply(invert(world_from_surface),
world_from_base)
#result = is_point_in_polygon(point_from_pose(surface_from_base), vertices_from_surface[surface_name])
#if not result:
# draw_pose(surface_from_base)
# points = [Point(x, y, 0) for x, y, in vertices_from_surface[surface_name]]
# add_segments(points, closed=True)
# wait_for_user()
return is_point_in_polygon(point_from_pose(surface_from_base),
vertices_from_surface[surface_name])
else:
if not base_from_objects:
return False
base_conf.assign()
pose.assign()
world_from_base = get_link_pose(world.robot, world.base_link)
world_from_object = pose.get_world_from_body()
base_from_object = multiply(invert(world_from_base),
world_from_object)
return is_point_in_polygon(point_from_pose(base_from_object),
base_from_objects)
def create_inverse_reachability(robot, body, table, arm, grasp_type, num_samples=500):
link = get_gripper_link(robot, arm)
movable_joints = get_movable_joints(robot)
default_conf = get_joint_positions(robot, movable_joints)
gripper_from_base_list = []
grasps = GET_GRASPS[grasp_type](body)
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))
set_joint_positions(robot, movable_joints, default_conf)
base_conf = next(uniform_pose_generator(robot, gripper_pose))
set_base_values(robot, base_conf)
if pairwise_collision(robot, table):
continue
conf = inverse_kinematics(robot, link, gripper_pose)
if (conf is None) or pairwise_collision(robot, table):
continue
gripper_from_base = multiply(invert(get_link_pose(robot, link)), get_pose(robot))
gripper_from_base_list.append(gripper_from_base)
print('{} / {}'.format(len(gripper_from_base_list), num_samples))
filename = IR_FILENAME.format(grasp_type, arm)
path = get_database_file(filename)
data = {
'filename': filename,
'robot': get_body_name(robot),
'grasp_type': grasp_type,
'arg': arm,
'carry_conf': get_carry_conf(arm, grasp_type),
'gripper_link': link,
'gripper_from_base': gripper_from_base_list,
}
write_pickle(path, data)
return path
def solve_trac_ik(self, world_from_tool, nearby_tolerance=INF):
assert self.ik_solver is not None
init_lower, init_upper = self.ik_solver.get_joint_limits()
base_link = link_from_name(self.robot, self.ik_solver.base_link)
world_from_base = get_link_pose(self.robot, base_link)
tip_link = link_from_name(self.robot, self.ik_solver.tip_link)
tool_from_tip = multiply(
invert(get_link_pose(self.robot, self.tool_link)),
get_link_pose(self.robot, tip_link))
world_from_tip = multiply(world_from_tool, tool_from_tip)
base_from_tip = multiply(invert(world_from_base), world_from_tip)
joints = joints_from_names(
self.robot,
self.ik_solver.joint_names) # self.ik_solver.link_names
seed_state = get_joint_positions(self.robot, joints)
# seed_state = [0.0] * self.ik_solver.number_of_joints
lower, upper = init_lower, init_upper
if nearby_tolerance < INF:
tolerance = nearby_tolerance * np.ones(len(joints))
lower = np.maximum(lower, seed_state - tolerance)
upper = np.minimum(upper, seed_state + tolerance)
self.ik_solver.set_joint_limits(lower, upper)
(x, y, z), (rx, ry, rz, rw) = base_from_tip
# TODO: can also adjust tolerances
conf = self.ik_solver.get_ik(seed_state, x, y, z, rx, ry, rz, rw)
self.ik_solver.set_joint_limits(init_lower, init_upper)
if conf is None:
return conf
# if nearby_tolerance < INF:
# print(lower.round(3))
# print(upper.round(3))
# print(conf)
# print(get_difference(seed_state, conf).round(3))
set_joint_positions(self.robot, joints, conf)
return get_configuration(self.robot)
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 gen(knob_name):
obstacles = world.static_obstacles
knob_link = link_from_name(world.kitchen, knob_name)
pose = get_link_pose(world.kitchen, knob_link)
#pose = RelPose(world.kitchen, knob_link, init=True)
presses = cycle(get_grasp_presses(world, knob_name))
grasp = next(presses)
gripper_pose = multiply(pose, invert(
grasp.grasp_pose)) # w_f_g = w_f_o * (g_f_o)^-1
if learned:
base_generator = cycle(load_pull_base_poses(world, knob_name))
else:
base_generator = uniform_pose_generator(world.robot, gripper_pose)
safe_base_generator = inverse_reachability(world,
base_generator,
obstacles=obstacles,
**kwargs)
while True:
for i in range(max_attempts):
try:
base_conf = next(safe_base_generator)
except StopIteration:
return
if base_conf is None:
yield None
continue
grasp = next(presses)
randomize = (random.random() < P_RANDOMIZE_IK)
ik_outputs = next(
plan_press(world,
knob_name,
pose,
grasp,
base_conf,
obstacles,
randomize=randomize,
**kwargs), None)
if ik_outputs is not None:
print('Press succeeded after {} attempts'.format(i))
yield (base_conf, ) + ik_outputs
break
else:
if PRINT_FAILURES:
print(
'Press failure after {} attempts'.format(max_attempts))
#if not pose.init:
# break
yield None
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)]