def add_body(self, name, **kwargs):
obj_type = type_from_name(name)
self.names_from_type.setdefault(obj_type, []).append(name)
path = get_obj_path(obj_type)
#self.path_from_name[name] = path
print('Loading', path)
body = load_pybullet(path, **kwargs)
assert body is not None
self.add(name, body)
def transition_belief_update(belief, plan):
if plan is None:
return False
success = True
for action, params in plan:
if action in ['move_base', 'calibrate', 'detect']:
pass
elif action == 'press-on':
s, k, o, bq, aq, gq, at = params
belief.pressed.add(k)
belief.cooked.add(o)
for bowl, liquid in belief.liquid:
if bowl == o:
belief.cooked.add(liquid)
elif action == 'press-off':
s, k, o, bq, aq, gq, at = params
belief.pressed.remove(k)
elif action == 'move_arm':
bq, aq1, aq2, at = params
belief.arm_conf = aq2
elif action == 'move_gripper':
gq1, gq2, gt = params
belief.gripper_conf = gq2
elif action == 'pull':
j, a1, a2, o, wp1, wp2, bq, aq1, aq2, gq, at = params
belief.door_confs[j] = a2
belief.arm_conf = aq2
elif action == 'pour':
bowl, wp, cup, g, liquid, bq, aq, at = params
belief.liquid.discard((cup, liquid))
belief.liquid.add((bowl, liquid))
elif action == 'pick':
o, p, g, rp = params[:4]
obj_type = type_from_name(o)
if (obj_type not in TIN_OBJECTS) or not belief.is_gripper_closed():
del belief.pose_dists[o]
belief.grasped = g
# TODO: open gripper afterwards to ensure not in hand
else:
delocalize_belief(belief, o, rp)
print('Failed to grasp! Delocalizing belief')
success = False
break
elif action == 'place':
o, p, g, rp = params[:4]
belief.grasped = None
if STOCHASTIC_PLACE and belief.world.is_real():
delocalize_belief(belief, o, rp)
else:
belief.pose_dists[o] = PoseDist(belief.world, o, DeltaDist(rp))
elif action == 'cook':
pass
else:
raise NotImplementedError(action)
# TODO: replan after every action
return success
def pour_path_from_parameter(world, bowl_name, cup_name):
bowl_body = world.get_body(bowl_name)
bowl_center, (bowl_d, bowl_h) = approximate_as_cylinder(bowl_body)
cup_body = world.get_body(cup_name)
cup_center, (cup_d, _, cup_h) = approximate_as_prism(cup_body)
#####
obj_type = type_from_name(cup_name)
if obj_type in [MUSTARD]:
initial_pitch = final_pitch = -np.pi
radius = 0
else:
initial_pitch = 0 # different if mustard
final_pitch = -3 * np.pi / 4
radius = bowl_d / 2
#axis_in_cup_center_x = -0.05
axis_in_cup_center_x = 0 # meters
#axis_in_cup_center_z = -cup_h/2.
axis_in_cup_center_z = 0. # meters
#axis_in_cup_center_z = +cup_h/2.
# tl := top left | tr := top right
cup_tl_in_center = np.array([-cup_d / 2, 0, cup_h / 2])
cup_tl_in_axis = cup_tl_in_center - Point(z=axis_in_cup_center_z)
cup_tl_angle = np.math.atan2(cup_tl_in_axis[2], cup_tl_in_axis[0])
cup_tl_pour_pitch = final_pitch - cup_tl_angle
cup_radius2d = np.linalg.norm([cup_tl_in_axis])
pivot_in_bowl_tr = Point(
x=-(cup_radius2d * np.math.cos(cup_tl_pour_pitch) + 0.01),
z=(cup_radius2d * np.math.sin(cup_tl_pour_pitch) + Z_OFFSET))
pivot_in_bowl_center = Point(x=radius, z=bowl_h / 2) + pivot_in_bowl_tr
base_from_pivot = Pose(
Point(x=axis_in_cup_center_x, z=axis_in_cup_center_z))
#####
assert -np.pi <= final_pitch <= initial_pitch
pitches = [initial_pitch]
if final_pitch != initial_pitch:
pitches = list(np.arange(final_pitch, initial_pitch,
np.pi / 16)) + pitches
cup_path_in_bowl = []
for pitch in pitches:
rotate_pivot = Pose(
euler=Euler(pitch=pitch)
) # Can also interpolate directly between start and end quat
cup_path_in_bowl.append(
multiply(Pose(point=bowl_center), Pose(pivot_in_bowl_center),
rotate_pivot, invert(base_from_pivot),
invert(Pose(point=cup_center))))
return cup_path_in_bowl
def is_gripper_closed(self):
# TODO: base this on the object type
if self.holding is not None:
obj_type = type_from_name(self.holding)
if obj_type not in TIN_OBJECTS:
return False
# each joint in [0.00, 0.04] (units coincide with meters on the physical gripper)
current_gq = get_joint_positions(self.world.robot,
self.world.gripper_joints)
gripper_width = sum(current_gq)
return gripper_width <= MIN_GRASP_WIDTH
def gen(bowl_name, wp, cup_name, grasp, bq):
# https://github.mit.edu/Learning-and-Intelligent-Systems/ltamp_pr2/blob/d1e6024c5c13df7edeab3a271b745e656a794b02/plan_tools/samplers/pour.py
if bowl_name == cup_name:
return
#attachment = get_grasp_attachment(world, arm, grasp)
bowl_body = world.get_body(bowl_name)
#cup_body = world.get_body(cup_name)
obstacles = (world.static_obstacles
| {bowl_body}) if collisions else set()
cup_path_bowl = pour_path_from_parameter(world, bowl_name, cup_name)
while True:
for _ in range(max_attempts):
bowl_pose = wp.get_world_from_body()
rotate_bowl = Pose(euler=Euler(
yaw=random.uniform(-np.pi, np.pi)))
rotate_cup = Pose(euler=Euler(
yaw=random.uniform(-np.pi, np.pi)))
cup_path = [
multiply(bowl_pose, invert(rotate_bowl), cup_pose_bowl,
rotate_cup) for cup_pose_bowl in cup_path_bowl
]
#visualize_cartesian_path(cup_body, cup_path)
#if cartesian_path_collision(cup_body, cup_path, obstacles + [bowl_body]):
# continue
tool_path = [
multiply(p, invert(grasp.grasp_pose)) for p in cup_path
]
# TODO: extra collision test for visibility
# TODO: orientation constraint while moving
bq.assign()
grasp.set_gripper()
world.carry_conf.assign()
arm_path = plan_workspace(world,
tool_path,
obstacles,
randomize=True) # tilt to upright
if arm_path is None:
continue
assert MOVE_ARM
aq = FConf(world.robot, world.arm_joints, arm_path[-1])
robot_saver = BodySaver(world.robot)
obj_type = type_from_name(cup_name)
duration = 5.0 if obj_type in [MUSTARD] else 1.0
objects = [bowl_name, cup_name]
cmd = Sequence(State(world, savers=[robot_saver]),
commands=[
ApproachTrajectory(objects, world,
world.robot,
world.arm_joints,
arm_path[::-1]),
Wait(world, duration=duration),
ApproachTrajectory(objects, world,
world.robot,
world.arm_joints,
arm_path),
],
name='pour')
yield (
aq,
cmd,
)
break
else:
yield None
def pdddlstream_from_problem(belief,
additional_init=[],
fixed_base=True,
**kwargs):
world = belief.world # One world per state
task = world.task # One task per world
print(task)
domain_pddl = read(get_file_path(__file__, '../pddl/domain.pddl'))
# TODO: repackage stream outputs to avoid recomputation
# Despite the base not moving, it could be re-estimated
init_bq = belief.base_conf
init_aq = belief.arm_conf
init_gq = belief.gripper_conf
carry_aq = world.carry_conf
init_aq = carry_aq if are_confs_close(init_aq, carry_aq) else init_aq
# TODO: the following doesn't work. Maybe because carry_conf is used elsewhere
#carry_aq = init_aq if are_confs_close(init_aq, world.carry_conf) else world.carry_conf
#calibrate_aq = init_aq if are_confs_close(init_aq, world.calibrate_conf) else world.calibrate_conf
# Don't need this now that returning to old confs
#open_gq = init_gq if are_confs_close(init_gq, world.open_gq) else world.open_gq
#closed_gq = init_gq if are_confs_close(init_gq, world.closed_gq) else world.closed_gq
open_gq = world.open_gq
closed_gq = world.closed_gq
constant_map = {
'@world': 'world',
'@gripper': 'gripper',
'@stove': 'stove',
'@none': None,
'@rest_aq': carry_aq,
#'@calibrate_aq': calibrate_aq,
'@open_gq': open_gq,
'@closed_gq': closed_gq,
'@open': OPEN,
'@closed': CLOSED,
'@top': TOP_GRASP,
'@side': SIDE_GRASP,
'@bq0': init_bq,
}
top_joint = JOINT_TEMPLATE.format(TOP_DRAWER)
bottom_joint = JOINT_TEMPLATE.format(BOTTOM_DRAWER)
init = [
('BConf', init_bq),
('AtBConf', init_bq),
('AConf', init_bq, carry_aq),
#('RestAConf', carry_aq),
#('AConf', init_bq, calibrate_aq),
(
'Stationary', ),
('AConf', init_bq, init_aq),
('AtAConf', init_aq),
('GConf', open_gq),
('GConf', closed_gq),
('Grasp', None, None),
('AtGrasp', None, None),
('Above', top_joint, bottom_joint),
('Adjacent', top_joint, bottom_joint),
('Adjacent', bottom_joint, top_joint),
('Calibrated', ),
('CanMoveBase', ),
('CanMoveArm', ),
('CanMoveGripper', ),
] + list(task.init) + list(additional_init)
for action_name, cost in ACTION_COSTS.items():
function_name = '{}Cost'.format(title_from_snake(action_name))
function = (function_name, )
init.append(Equal(function, cost)) # TODO: stove state
init += [('Stackable', name, surface) for name, surface in task.goal_on.items()] + \
[('Stackable', name, stove) for name, stove in product(task.goal_cooked, STOVES)] + \
[('Pressed', name) for name in belief.pressed] + \
[('Cookable', name) for name in task.goal_cooked] + \
[('Cooked', name) for name in belief.cooked] + \
[('Status', status) for status in DOOR_STATUSES] + \
[('Knob', knob) for knob in KNOBS] + \
[('Joint', knob) for knob in KNOBS] + \
[('Liquid', liquid) for _, liquid in task.init_liquid] + \
[('HasLiquid', cup, liquid) for cup, liquid in belief.liquid] + \
[('StoveKnob', STOVE_TEMPLATE.format(loc), KNOB_TEMPLATE.format(loc)) for loc in STOVE_LOCATIONS] + \
[('GraspType', ty) for ty in task.grasp_types] # TODO: grasp_type per object
#[('Type', obj_name, 'stove') for obj_name in STOVES] + \
#[('Camera', name) for name in world.cameras]
if task.movable_base:
init.append(('MovableBase', ))
if fixed_base:
init.append(('InitBConf', init_bq))
if task.noisy_base:
init.append(('NoisyBase', ))
compute_pose_kin = get_compute_pose_kin(world)
compute_angle_kin = get_compute_angle_kin(world)
initial_poses = {}
for joint_name, init_conf in belief.door_confs.items():
if joint_name in DRAWER_JOINTS:
init.append(('Drawer', joint_name))
if joint_name in CABINET_JOINTS:
init.append(('Cabinet', joint_name))
joint = joint_from_name(world.kitchen, joint_name)
surface_name = surface_from_joint(joint_name)
init.append(('SurfaceJoint', surface_name, joint_name))
# Relies on the fact that drawers have identical surface and link names
link_name = get_link_name(world.kitchen, child_link_from_joint(joint))
#link_name = str(link_name.decode('UTF-8'))
#link_name = str(link_name.encode('ascii','ignore'))
for conf in {
init_conf, world.open_kitchen_confs[joint],
world.closed_kitchen_confs[joint]
}:
# TODO: return to initial poses?
world_pose, = compute_angle_kin(link_name, joint_name, conf)
init.extend([
('Joint', joint_name),
('Angle', joint_name, conf),
('Obstacle', link_name),
('AngleKin', link_name, world_pose, joint_name, conf),
('WorldPose', link_name, world_pose),
])
if joint in world.kitchen_joints:
init.extend([
('Sample', world_pose),
#('Value', world_pose), # comment out?
])
if conf == init_conf:
initial_poses[link_name] = world_pose
init.extend([
('AtAngle', joint_name, conf),
('AtWorldPose', link_name, world_pose),
])
for surface_name in ALL_SURFACES:
if surface_name in OPEN_SURFACES:
init.append(('Counter', surface_name)) # Fixed surface
if surface_name in DRAWERS:
init.append(('Drawer', surface_name))
surface = surface_from_name(surface_name)
surface_link = link_from_name(world.kitchen, surface.link)
parent_joint = parent_joint_from_link(surface_link)
if parent_joint not in world.kitchen_joints:
# TODO: attach to world frame?
world_pose = RelPose(world.kitchen, surface_link, init=True)
initial_poses[surface_name] = world_pose
init += [
#('RelPose', surface_name, world_pose, 'world'),
('WorldPose', surface_name, world_pose),
#('AtRelPose', surface_name, world_pose, 'world'),
('AtWorldPose', surface_name, world_pose),
('Sample', world_pose),
#('Value', world_pose),
]
init.extend([
('CheckNearby', surface_name),
#('InitPose', world_pose),
('Localized', surface_name),
])
for grasp_type in task.grasp_types:
if (surface_name in OPEN_SURFACES) or has_place_database(
world.robot_name, surface_name, grasp_type):
init.append(('AdmitsGraspType', surface_name, grasp_type))
if belief.grasped is None:
init.extend([
('HandEmpty', ),
('GConf', init_gq),
('AtGConf', init_gq),
])
else:
obj_name = belief.grasped.body_name
assert obj_name not in belief.pose_dists
grasp = belief.grasped
init += [
# Static
#('Graspable', obj_name),
('Grasp', obj_name, grasp),
('IsGraspType', obj_name, grasp, grasp.grasp_type),
# Fluent
('AtGrasp', obj_name, grasp),
('Holding', obj_name),
('Localized', obj_name),
]
init.extend(('ValidGraspType', obj_name, grasp_type)
for grasp_type in task.grasp_types
if implies(world.is_real(),
is_valid_grasp_type(obj_name, grasp_type)))
for obj_name in world.movable:
obj_type = type_from_name(obj_name)
if obj_type in BOWLS:
init.append(('Bowl', obj_name))
else:
init.append(
('Obstacle', obj_name)) # TODO: hack to place within bowls
if obj_type in COOKABLE:
init.append(('Cookable', obj_name))
if obj_type in POURABLE:
init.append(('Pourable', obj_name))
init += [
('Entity', obj_name),
('CheckNearby', obj_name),
] + [('Stackable', obj_name, counter)
for counter in set(ALL_SURFACES) & set(COUNTERS)]
# TODO: track poses over time to produce estimates
for obj_name, pose_dist in belief.pose_dists.items():
dist_support = pose_dist.dist.support()
localized = pose_dist.is_localized()
graspable = True
if localized:
init.append(('Localized', obj_name))
[rel_pose] = dist_support
roll, pitch, yaw = euler_from_quat(
quat_from_pose(rel_pose.get_reference_from_body()))
if (MAX_ERROR < abs(roll)) or (MAX_ERROR < abs(pitch)):
graspable = False
print(
'{} has an invalid orientation: roll={:.3f}, pitch={:.3f}'.
format(obj_name, roll, pitch))
if graspable:
#init.append(('Graspable', obj_name))
init.extend(('ValidGraspType', obj_name, grasp_type)
for grasp_type in task.grasp_types
if implies(world.is_real(),
is_valid_grasp_type(obj_name, grasp_type)))
# Could also fully decompose into points (but many samples)
# Could immediately add likely points for collision checking
for rel_pose in (dist_support if localized else pose_dist.decompose()):
surface_name = rel_pose.support
if surface_name is None:
# Treats as obstacle
# TODO: could temporarily add to fixed
world_pose = rel_pose
init += [
('WorldPose', obj_name, world_pose),
('AtWorldPose', obj_name, world_pose),
]
poses = [world_pose]
#raise RuntimeError(obj_name, supporting)
else:
surface_pose = initial_poses[surface_name]
world_pose, = compute_pose_kin(obj_name, rel_pose,
surface_name, surface_pose)
init += [
# Static
('RelPose', obj_name, rel_pose, surface_name),
('WorldPose', obj_name, world_pose),
('PoseKin', obj_name, world_pose, rel_pose, surface_name,
surface_pose),
# Fluent
('AtRelPose', obj_name, rel_pose, surface_name),
('AtWorldPose', obj_name, world_pose),
]
if localized:
init.append(('On', obj_name, surface_name))
poses = [rel_pose, world_pose]
for pose in poses:
if isinstance(pose, PoseDist):
init.append(('Dist', pose))
else:
init.extend([('Sample', pose)]) #, ('Value', pose)])
#for body, ty in problem.body_types:
# init += [('Type', body, ty)]
#bodies_from_type = get_bodies_from_type(problem)
#bodies = bodies_from_type[get_parameter_name(ty)] if is_parameter(ty) else [ty]
goal_formula = get_goal(belief, init)
stream_pddl, stream_map = get_streams(world,
teleport_base=task.teleport_base,
**kwargs)
print('Constants:', constant_map)
print('Init:', sorted(init, key=lambda f: f[0]))
print('Goal:', goal_formula)
#print('Streams:', stream_map.keys()) # DEBUG
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal_formula)