def __repr__(self):
return '{}(robot={}, gripper={}, movable={}, surfaces={})'.format(
self.__class__.__name__, get_model_name(self.mbp, self.robot),
get_model_name(self.mbp, self.gripper),
[get_model_name(self.mbp, model) for model in self.movable],
self.surfaces)
def __repr__(self):
return '{}({}->{})'.format(self.__class__.__name__,
get_model_name(self.mbp, self.child),
self.parent.name())
def __repr__(self):
return '{}({},{},{})'.format(
self.__class__.__name__,
get_model_name(self.plant, self.model_index), self.body_name,
self.visual_index)
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)