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 get_pose_gen(task, context, collisions=True, shrink=0.025):
mbp = task.mbp
world = mbp.world_frame()
box_from_geom = get_box_from_geom(task.scene_graph)
fixed = task.fixed_bodies() if collisions else []
world_from_robot = get_world_pose(task.mbp, context, task.robot)
max_xy_distance = 0.85
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, )
return gen
def load_station(time_step=0.0, **kwargs):
station = ManipulationStation(time_step, IiwaCollisionModel.kBoxCollision)
plant = station.get_mutable_multibody_plant()
scene_graph = station.get_mutable_scene_graph()
station.AddCupboard()
robot = plant.GetModelInstanceByName('iiwa')
gripper = plant.GetModelInstanceByName('gripper')
table = plant.GetModelInstanceByName('table')
cupboard = plant.GetModelInstanceByName('cupboard')
model_name = 'soup'
item = AddModelFromSdfFile(file_name=get_sdf_path(model_name),
model_name=model_name,
plant=plant,
scene_graph=scene_graph)
ceiling = AddModelFromSdfFile(file_name=PLANE_FILE_PATH,
model_name="ceiling",
plant=plant,
scene_graph=scene_graph)
weld_to_world(plant, ceiling,
create_transform(translation=[0.3257, 0, 1.0]))
station.Finalize()
diagram, state_machine = build_manipulation_station(station, **kwargs)
box_from_geom = get_box_from_geom(scene_graph)
table_body = plant.GetBodyByName('amazon_table', table)
table_index = 0
#table_surface = Surface(plant, table, table_body.name(), table_index),
start_z = get_z_placement(plant, box_from_geom, item, table_body,
table_index)
shelf_body = plant.GetBodyByName('top_and_bottom', cupboard)
shelf_index = CUPBOARD_SHELVES.index('shelf_lower')
goal_surface = Surface(plant, cupboard, shelf_body.name(), shelf_index)
door_names = [
'left_door',
#'right_door',
]
doors = [plant.GetBodyByName(name).index() for name in door_names]
initial_positions = {
plant.GetJointByName('left_door_hinge'): -DOOR_CLOSED,
plant.GetJointByName('right_door_hinge'): DOOR_CLOSED,
}
initial_conf = [0, 0, 0, -1.75, 0, 1.0, 0]
initial_positions.update(
zip(get_movable_joints(plant, robot), initial_conf))
start_x, start_y, start_theta = 0.4, -0.2, np.pi / 2
initial_poses = {
item:
create_transform(translation=[start_x, start_y, start_z],
rotation=[0, 0, start_theta]),
}
surfaces = [
#table_surface,
goal_surface,
]
task = Task(
diagram,
plant,
scene_graph,
robot,
gripper,
movable=[item],
surfaces=surfaces,
doors=doors,
initial_positions=initial_positions,
initial_poses=initial_poses,
#goal_holding=[item],
goal_on=[(item, goal_surface)],
#goal_poses=goal_poses,
reset_robot=True,
reset_doors=False)
task.set_initial()
task.station = station
return task, diagram, state_machine
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