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
def fn(robot_name, door_name, door_conf1, door_conf2):
robot = task.mbp.GetModelInstanceByName(robot_name)
robot_joints = get_movable_joints(task.mbp, robot)
door_body = task.mbp.GetBodyByName(door_name)
door_joints = door_conf1.joints
combined_joints = robot_joints + door_joints
if not np.allclose(get_door_positions(door_body, DOOR_CLOSED),
door_conf1.positions):
return
combined_waypoints = [
list(home_conf) + list(door_conf.positions)
for door_conf in [door_conf1, door_conf2]
]
combined_joint_path = plan_waypoints_joint_motion(
combined_joints, combined_waypoints, collision_fn=lambda q: False)
if combined_joint_path is None:
return
robot_conf1 = Conf(robot_joints, home_conf)
robot_conf2 = robot_conf1
traj = Trajectory([
Conf(combined_joints, combined_conf)
for combined_conf in combined_joint_path
],
force_control=True)
yield (robot_conf1, robot_conf2, traj)
def get_open_trajectory(plant, gripper):
gripper_joints = get_movable_joints(plant, gripper)
gripper_extend_fn = get_extend_fn(gripper_joints)
gripper_closed_conf = get_close_wsg50_positions(plant, gripper)
gripper_path = list(
gripper_extend_fn(gripper_closed_conf,
get_open_wsg50_positions(plant, gripper)))
gripper_path.insert(0, gripper_closed_conf)
return Trajectory(Conf(gripper_joints, q) for q in gripper_path)
def fn(robot_name, conf1, conf2, fluents=[]):
robot = task.mbp.GetModelInstanceByName(robot_name)
joints = get_movable_joints(task.mbp, robot)
moving = bodies_from_models(task.mbp, [robot, gripper])
obstacles = set(task.fixed_bodies())
attachments = parse_fluents(fluents, context, obstacles)
for grasp in attachments:
moving.update(grasp.bodies)
obstacles -= moving
if teleport:
traj = Trajectory([conf1, conf2], attachments=attachments)
return (traj, )
collision_pairs = set(product(moving,
obstacles)) if collisions else set()
collision_fn = get_collision_fn(task.diagram,
task.diagram_context,
task.mbp,
task.scene_graph,
joints,
collision_pairs=collision_pairs,
attachments=attachments)
#sample_fn = get_sample_fn(joints, start_conf=conf1.positions,
# end_conf=conf2.positions, collision_fn=collision_fn)
sample_fn = None
open_wsg50_gripper(task.mbp, context, gripper)
path = plan_joint_motion(joints,
conf1.positions,
conf2.positions,
sample_fn=sample_fn,
collision_fn=collision_fn,
restarts=10,
iterations=75,
smooth=50)
#restarts=25, iterations=25, smooth=0) # Disabled smoothing to see the path
if path is None:
return None
# Sample within the ellipsoid
traj = Trajectory([Conf(joints, q) for q in path],
attachments=attachments)
return (traj, )
def open_wsg50_gripper(mbp, context, model_index):
set_joint_positions(get_movable_joints(mbp, model_index), context,
get_open_wsg50_positions(mbp, model_index))
def close_wsg50_gripper(mbp, context, model_index): # 0.05
set_joint_positions(get_movable_joints(mbp, model_index), context,
get_close_wsg50_positions(mbp, model_index))
def load_dope(time_step=0.0,
dope_path=DOPE_PATH,
goal_name='soup',
is_visualizing=True,
**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')
cupboard = plant.GetModelInstanceByName('cupboard')
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.1]))
pose_from_name = read_poses_from_file(dope_path)
model_from_name = {}
for name in pose_from_name:
model_from_name[name] = AddModelFromSdfFile(
file_name=get_sdf_path(name),
model_name=name,
plant=plant,
scene_graph=scene_graph)
station.Finalize()
door_names = [
'left_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))
initial_poses = {
model_from_name[name]: pose_from_name[name]
for name in pose_from_name
}
goal_shelf = 'shelf_lower'
print('Goal shelf:', goal_shelf)
goal_surface = Surface(plant, cupboard, 'top_and_bottom',
CUPBOARD_SHELVES.index(goal_shelf))
surfaces = [
goal_surface,
]
item = model_from_name[goal_name]
diagram, state_machine = build_manipulation_station(
station, visualize=is_visualizing, **kwargs)
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)],
reset_robot=True,
reset_doors=False)
task.set_initial()
task.station = station
return task, diagram, state_machine
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 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)
def get_pddlstream_problem(task,
context,
collisions=True,
use_impedance=False):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
plant = task.mbp
robot = task.robot
robot_name = get_model_name(plant, robot)
world = plant.world_frame() # mbp.world_body()
robot_joints = get_movable_joints(plant, robot)
robot_conf = Conf(robot_joints, get_configuration(plant, context, robot))
init = [
('Robot', robot_name),
('CanMove', robot_name),
('Conf', robot_name, robot_conf),
('AtConf', robot_name, robot_conf),
('HandEmpty', robot_name),
]
goal_literals = []
if task.reset_robot:
goal_literals.append(('AtConf', robot_name, robot_conf), )
for obj in task.movable:
obj_name = get_model_name(plant, obj)
obj_pose = Pose(plant, world, obj, get_world_pose(plant, context, obj))
init += [('Graspable', obj_name), ('Pose', obj_name, obj_pose),
('InitPose', obj_name, obj_pose),
('AtPose', obj_name, obj_pose)]
for surface in task.surfaces:
init += [('Stackable', obj_name, surface)]
for surface in task.surfaces:
surface_name = get_model_name(plant, surface.model_index)
if 'sink' in surface_name:
init += [('Sink', surface)]
if 'stove' in surface_name:
init += [('Stove', surface)]
for door in task.doors:
door_body = plant.tree().get_body(door)
door_name = door_body.name()
door_joints = get_parent_joints(plant, door_body)
door_conf = Conf(door_joints,
get_joint_positions(door_joints, context))
init += [
('Door', door_name),
('Conf', door_name, door_conf),
('AtConf', door_name, door_conf),
]
for positions in [get_door_positions(door_body, DOOR_OPEN)]:
conf = Conf(door_joints, positions)
init += [('Conf', door_name, conf)]
if task.reset_doors:
goal_literals += [('AtConf', door_name, door_conf)]
for obj, transform in task.goal_poses.items():
obj_name = get_model_name(plant, obj)
obj_pose = Pose(plant, world, obj, transform)
init += [
('Pose', obj_name, obj_pose),
]
goal_literals.append(('AtPose', obj_name, obj_pose))
for obj in task.goal_holding:
goal_literals.append(
('Holding', robot_name, get_model_name(plant, obj)))
for obj, surface in task.goal_on:
goal_literals.append(('On', get_model_name(plant, obj), surface))
for obj in task.goal_cooked:
goal_literals.append(('Cooked', get_model_name(plant, obj)))
goal = And(*goal_literals)
print('Initial:', init)
print('Goal:', goal)
stream_map = {
'sample-reachable-grasp':
from_gen_fn(
get_reachable_grasp_gen_fn(task, context, collisions=collisions)),
'sample-reachable-pose':
from_gen_fn(
get_reachable_pose_gen_fn(task, context, collisions=collisions)),
'plan-ik':
from_gen_fn(get_ik_gen_fn(task, context, collisions=collisions)),
'plan-motion':
from_fn(get_motion_fn(task, context, collisions=collisions)),
'TrajPoseCollision':
get_collision_test(task, context, collisions=collisions),
'TrajConfCollision':
get_collision_test(task, context, collisions=collisions),
}
if use_impedance:
stream_map['plan-pull'] = from_gen_fn(
get_force_pull_fn(task, context, collisions=collisions))
else:
stream_map['plan-pull'] = from_gen_fn(
get_pull_fn(task, context, collisions=collisions))
#stream_map = 'debug' # Runs PDDLStream with "placeholder streams" for debugging
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)