def get_grasp_gen_fn(task):
plant = task.mbp
gripper_frame = get_base_body(plant, task.gripper).body_frame()
box_from_geom = get_box_from_geom(task.scene_graph)
pitch = 4 * np.pi / 9
assert abs(pitch) <= np.pi / 2
def gen(obj_name):
obj = plant.GetModelInstanceByName(obj_name)
obj_aabb, obj_from_box, obj_shape = box_from_geom[
int(obj), get_base_body(plant, obj).name(), 0]
if obj_shape == 'cylinder':
grasp_gen = get_cylinder_grasps(obj_aabb,
pitch_range=(pitch, pitch))
elif obj_shape == 'box':
grasp_gen = get_box_grasps(obj_aabb, pitch_range=(pitch, pitch))
else:
raise NotImplementedError(obj_shape)
for gripper_from_box in grasp_gen:
gripper_from_obj = gripper_from_box.multiply(
obj_from_box.inverse())
grasp = Pose(plant, gripper_frame, obj, gripper_from_obj)
yield (grasp, )
return gen
def gen(obj_name, surface):
obj = mbp.GetModelInstanceByName(obj_name)
obj_aabb, obj_from_box, _ = box_from_geom[
int(obj), get_base_body(mbp, obj).name(), 0]
collision_pairs = set(product(get_model_bodies(mbp, obj), fixed))
surface_body = mbp.GetBodyByName(surface.body_name,
surface.model_index)
surface_pose = get_body_pose(context, surface_body)
surface_aabb, surface_from_box, _ = box_from_geom[
int(surface.model_index), surface.body_name, surface.visual_index]
for surface_box_from_obj_box in sample_aabb_placement(obj_aabb,
surface_aabb,
shrink=shrink):
world_from_obj = surface_pose.multiply(surface_from_box).multiply(
surface_box_from_obj_box).multiply(obj_from_box.inverse())
robot_from_obj = world_from_robot.inverse().multiply(
world_from_obj)
if max_xy_distance < np.linalg.norm(
robot_from_obj.translation()[:2], ord=np.inf):
continue
pose = Pose(mbp, world, obj, world_from_obj, surface=surface)
pose.assign(context)
if not exists_colliding_pair(task.diagram, task.diagram_context,
task.mbp, task.scene_graph,
collision_pairs):
yield (pose, )
def get_z_placement(plant, box_from_geom, item_body, surface_body,
surface_index):
plant_context = plant.CreateDefaultContext()
surface_aabb, surface_body_from_box, _ = get_body_boxes(
surface_body, box_from_geom)[surface_index]
[(item_aabb, item_body_from_box, _)
] = get_body_boxes(get_base_body(plant, item_body), box_from_geom)
dz = get_aabb_z_placement(item_aabb, surface_aabb, z_epsilon=0)
surface_box_from_obj_box = create_transform(translation=[0, 0, dz])
world_pose = get_body_pose(plant_context, surface_body).multiply(
surface_body_from_box).multiply(surface_box_from_obj_box).multiply(
item_body_from_box.inverse())
_, _, item_z = world_pose.translation()
return item_z
def gen(obj_name):
obj = plant.GetModelInstanceByName(obj_name)
obj_aabb, obj_from_box, obj_shape = box_from_geom[
int(obj), get_base_body(plant, obj).name(), 0]
if obj_shape == 'cylinder':
grasp_gen = get_cylinder_grasps(obj_aabb,
pitch_range=(pitch, pitch))
elif obj_shape == 'box':
grasp_gen = get_box_grasps(obj_aabb, pitch_range=(pitch, pitch))
else:
raise NotImplementedError(obj_shape)
for gripper_from_box in grasp_gen:
gripper_from_obj = gripper_from_box.multiply(
obj_from_box.inverse())
grasp = Pose(plant, gripper_frame, obj, gripper_from_obj)
yield (grasp, )
def get_pull_fn(task,
context,
collisions=True,
max_attempts=25,
step_size=np.pi / 16,
approach_distance=0.05):
box_from_geom = get_box_from_geom(task.scene_graph)
gripper_frame = get_base_body(task.mbp, task.gripper).body_frame()
fixed = task.fixed_bodies() if collisions else []
def fn(robot_name, door_name, door_conf1, door_conf2):
"""
:param robot_name: The name of the robot (should be iiwa)
:param door_name: The name of the door (should be left_door or right_door)
:param door_conf1: The initial door configuration
:param door_conf2: The final door configuration
:return: A triplet composed of the initial robot configuration, final robot configuration,
and combined robot & door position trajectory to execute the pull
"""
robot = task.mbp.GetModelInstanceByName(robot_name)
robot_joints = get_movable_joints(task.mbp, robot)
collision_pairs = set(
product(bodies_from_models(task.mbp, [robot, task.gripper]),
fixed))
collision_fn = get_collision_fn(task.diagram,
task.diagram_context,
task.mbp,
task.scene_graph,
robot_joints,
collision_pairs=collision_pairs)
door_body = task.mbp.GetBodyByName(door_name)
door_joints = door_conf1.joints
combined_joints = robot_joints + door_joints
# The transformation from the door frame to the gripper frame that corresponds to grasping the door handle
gripper_from_door = get_door_grasp(door_body, box_from_geom)
extend_fn = get_extend_fn(door_joints,
resolutions=step_size *
np.ones(len(door_joints)))
door_joint_path = [door_conf1.positions] + list(
extend_fn(door_conf1.positions, door_conf2.positions))
door_body_path = get_body_path(door_body, context, door_joints,
door_joint_path)
gripper_body_path = [
door_pose.multiply(gripper_from_door.inverse())
for door_pose in door_body_path
]
for _ in range(max_attempts):
robot_joint_waypoints = plan_workspace_motion(
task.mbp,
robot_joints,
gripper_frame,
gripper_body_path,
collision_fn=collision_fn)
if robot_joint_waypoints is None:
continue
combined_waypoints = [
list(rq) + list(dq)
for rq, dq in zip(robot_joint_waypoints, door_joint_path)
]
combined_joint_path = plan_waypoints_joint_motion(
combined_joints,
combined_waypoints,
collision_fn=lambda q: False)
if combined_joint_path is None:
continue
# combined_joint_path is a joint position path for the concatenated robot and door joints.
# It should be a list of 8 DOF configurations (7 robot DOFs + 1 door DOF).
# Additionally, combined_joint_path[0][len(robot_joints):] should equal door_conf1.positions
# and combined_joint_path[-1][len(robot_joints):] should equal door_conf2.positions.
robot_conf1 = Conf(robot_joints,
combined_joint_path[0][:len(robot_joints)])
robot_conf2 = Conf(robot_joints,
combined_joint_path[-1][:len(robot_joints)])
traj = Trajectory(
Conf(combined_joints, combined_conf)
for combined_conf in combined_joint_path)
yield (robot_conf1, robot_conf2, traj)
return fn
def get_ik_gen_fn(task,
context,
collisions=True,
max_failures=10,
step_size=0.05):
approach_vector = 0.15 * np.array([0, -1, 0])
world_vector = 0.05 * np.array([0, 0, +1])
gripper_frame = get_base_body(task.mbp, task.gripper).body_frame()
door_bodies = {
task.mbp.tree().get_body(door_index)
for door_index in task.doors
}
fixed = (task.fixed_bodies() | door_bodies) if collisions else []
initial_guess = get_state(task.mbp, context)[:task.mbp.num_positions()]
# Above shelves prevent some placements
def fn(robot_name, obj_name, obj_pose, obj_grasp):
robot = task.mbp.GetModelInstanceByName(robot_name)
joints = get_movable_joints(task.mbp, robot)
collision_pairs = set(
product(bodies_from_models(task.mbp, [robot, task.gripper]),
fixed))
collision_fn = get_collision_fn(
task.diagram,
task.diagram_context,
task.mbp,
task.scene_graph,
joints,
collision_pairs=collision_pairs) # TODO: while holding
gripper_pose = obj_pose.transform.multiply(
obj_grasp.transform.inverse())
end_position = gripper_pose.multiply(approach_vector) + world_vector
translation = end_position - gripper_pose.translation()
gripper_path = list(
interpolate_translation(gripper_pose,
translation,
step_size=step_size))
attempts = 0
last_success = 0
while (attempts - last_success) < max_failures:
attempts += 1
obj_pose.assign(context)
if door_bodies:
for door_body in door_bodies:
positions = get_door_positions(door_body, DOOR_OPEN)
for door_joint in get_movable_joints(
task.plant, door_body.model_instance()):
if door_joint.child_body() == door_body:
set_joint_positions([door_joint], context,
positions)
path = plan_frame_motion(task.plant,
joints,
gripper_frame,
gripper_path,
initial_guess=initial_guess,
collision_fn=collision_fn)
if path is None:
continue
traj = Trajectory([Conf(joints, q) for q in path],
attachments=[obj_grasp])
conf = traj.path[-1]
yield (conf, traj)
last_success = attempts
return fn