def test_grasps(robot, block):
for arm in ARMS:
gripper_joints = get_gripper_joints(robot, arm)
tool_link = link_from_name(robot, TOOL_LINK.format(arm))
tool_pose = get_link_pose(robot, tool_link)
#handles = draw_pose(tool_pose)
#grasps = get_top_grasps(block, under=True, tool_pose=unit_pose())
grasps = get_side_grasps(block, under=True, tool_pose=unit_pose())
for i, grasp_pose in enumerate(grasps):
block_pose = multiply(tool_pose, grasp_pose)
set_pose(block, block_pose)
close_until_collision(robot, gripper_joints, bodies=[block], open_conf=get_open_positions(robot, arm),
closed_conf=get_closed_positions(robot, arm))
handles = draw_pose(block_pose)
wait_if_gui('Grasp {}'.format(i))
remove_handles(handles)
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 compute_grasp_width(robot, arm, body, grasp_pose, **kwargs):
gripper_joints = get_gripper_joints(robot, arm)
tool_link = link_from_name(robot, TOOL_FRAMES[arm])
tool_pose = get_link_pose(robot, tool_link)
body_pose = multiply(tool_pose, grasp_pose)
set_pose(body, body_pose)
return close_until_collision(robot,
gripper_joints,
bodies=[body],
max_distance=0.0,
**kwargs)
def plan_pull(world,
door_joint,
door_plan,
base_conf,
randomize=True,
collisions=True,
teleport=False,
**kwargs):
door_path, handle_path, handle_plan, tool_path = door_plan
handle_link, handle_grasp, handle_pregrasp = handle_plan
door_obstacles = get_descendant_obstacles(
world.kitchen, door_joint) # if collisions else set()
obstacles = (world.static_obstacles | door_obstacles
) # if collisions else set()
base_conf.assign()
world.open_gripper()
world.carry_conf.assign()
robot_saver = BodySaver(world.robot) # TODO: door_saver?
if not is_pull_safe(world, door_joint, door_plan):
return
arm_path = plan_workspace(world,
tool_path,
world.static_obstacles,
randomize=randomize,
teleport=collisions)
if arm_path is None:
return
approach_paths = []
for index in [0, -1]:
set_joint_positions(world.kitchen, [door_joint], door_path[index])
set_joint_positions(world.robot, world.arm_joints, arm_path[index])
tool_pose = multiply(handle_path[index], invert(handle_pregrasp))
approach_path = plan_approach(world,
tool_pose,
obstacles=obstacles,
teleport=teleport,
**kwargs)
if approach_path is None:
return
approach_paths.append(approach_path)
if MOVE_ARM:
aq1 = FConf(world.robot, world.arm_joints, approach_paths[0][0])
aq2 = FConf(world.robot, world.arm_joints, approach_paths[-1][0])
else:
aq1 = world.carry_conf
aq2 = aq1
set_joint_positions(world.kitchen, [door_joint], door_path[0])
set_joint_positions(world.robot, world.arm_joints, arm_path[0])
grasp_width = close_until_collision(world.robot,
world.gripper_joints,
bodies=[(world.kitchen, [handle_link])
])
gripper_motion_fn = get_gripper_motion_gen(world,
teleport=teleport,
collisions=collisions,
**kwargs)
gripper_conf = FConf(world.robot, world.gripper_joints,
[grasp_width] * len(world.gripper_joints))
finger_cmd, = gripper_motion_fn(world.open_gq, gripper_conf)
objects = []
commands = [
ApproachTrajectory(objects, world, world.robot, world.arm_joints,
approach_paths[0]),
DoorTrajectory(world, world.robot, world.arm_joints, arm_path,
world.kitchen, [door_joint], door_path),
ApproachTrajectory(objects, world, world.robot, world.arm_joints,
reversed(approach_paths[-1])),
]
door_path, _, _, _ = door_plan
sign = world.get_door_sign(door_joint)
pull = (sign * door_path[0][0] < sign * door_path[-1][0])
if pull:
commands.insert(1, finger_cmd.commands[0])
commands.insert(3, finger_cmd.commands[0].reverse())
cmd = Sequence(State(world, savers=[robot_saver]), commands, name='pull')
yield (
aq1,
aq2,
cmd,
)