def convert_splines(mbp, robot, gripper, context, trajectories):
# TODO: move to trajectory class
print()
splines, gripper_setpoints = [], []
for i, traj in enumerate(trajectories):
traj.path[-1].assign(context)
joints = traj.path[0].joints
if len(joints) == 8: # TODO: fix this
joints = joints[:7]
if len(joints) == 2:
q_knots_kuka = np.zeros((2, 7))
q_knots_kuka[0] = get_configuration(mbp, context, robot) # Second is velocity
splines.append(PiecewisePolynomial.ZeroOrderHold([0, 1], q_knots_kuka.T))
elif len(joints) == 7:
# TODO: adjust timing based on distance & velocities
# TODO: adjust number of waypoints
distance_fn = get_distance_fn(joints)
#path = [traj.path[0].positions, traj.path[-1].positions]
path = [q.positions[:len(joints)] for q in traj.path]
path = waypoints_from_path(joints, path) # TODO: increase time for pick/place & hold
q_knots_kuka = np.vstack(path).T
distances = [0.] + [distance_fn(q1, q2) for q1, q2 in zip(path, path[1:])]
t_knots = np.cumsum(distances) / RADIANS_PER_SECOND # TODO: this should be a max
d, n = q_knots_kuka.shape
print('{}) d={}, n={}, duration={:.3f}'.format(i, d, n, t_knots[-1]))
splines.append(PiecewisePolynomial.Cubic(
breaks=t_knots,
knots=q_knots_kuka,
knot_dot_start=np.zeros(d),
knot_dot_end=np.zeros(d)))
# RuntimeError: times must be in increasing order.
else:
raise ValueError(joints)
_, gripper_setpoint = get_configuration(mbp, context, gripper)
gripper_setpoints.append(gripper_setpoint)
return splines, gripper_setpoints
def convert_splines(mbp, robot, gripper, context, trajectories):
# TODO: move to trajectory class
print
splines, gripper_setpoints = [], []
for i, traj in enumerate(trajectories):
traj.path[-1].assign(context)
joints = traj.joints
if len(joints) == 8: # TODO: remove inclusion of door joints
joints = joints[:7]
if len(joints) == 2:
spline = get_hold_spline(mbp, context, robot)
elif len(joints) == 7:
spline = traj.spline()
else:
raise ValueError(joints)
d = spline.rows()
n = spline.get_number_of_segments()
print('{}) d={}, n={}, duration={:.3f}'.format(i, d, n, spline.duration(n-1)))
splines.append(spline)
_, gripper_setpoint = get_configuration(mbp, context, gripper)
gripper_setpoints.append(gripper_setpoint)
return splines, gripper_setpoints
def get_pddlstream_problem(task, context, collisions=True):
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_frame = get_base_body(mbp, obj).body_frame()
obj_pose = Pose(plant, world, obj,
get_world_pose(plant, context,
obj)) # get_relative_transform
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)]
# TODO: detect already stacked
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-pose': from_gen_fn(get_stable_gen(task, context, collisions=collisions)),
'sample-reachable-pose':
from_gen_fn(
get_reachable_pose_gen_fn(task, context, collisions=collisions)),
'sample-grasp':
from_gen_fn(get_grasp_gen_fn(task)),
'plan-ik':
from_gen_fn(get_ik_gen_fn(task, context, collisions=collisions)),
'plan-motion':
from_fn(get_motion_fn(task, context, collisions=collisions)),
'plan-pull':
from_fn(get_pull_fn(task, context, collisions=collisions)),
'TrajPoseCollision':
get_collision_test(task, context, collisions=collisions),
'TrajConfCollision':
get_collision_test(task, context, collisions=collisions),
}
#stream_map = 'debug' # Runs PDDLStream with "placeholder streams" for debugging
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def get_hold_spline(mbp, context, robot):
q_knots_kuka = np.zeros((2, 7))
q_knots_kuka[0] = get_configuration(mbp, context, robot) # Second is velocity
return PiecewisePolynomial.ZeroOrderHold([0, 1], q_knots_kuka.T)
def get_pddlstream_problem(mbp, context, scene_graph, task, collisions=True):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
robot = task.robot
#gripper = task.gripper
robot_name = get_model_name(mbp, robot)
world = mbp.world_frame() # mbp.world_body()
robot_joints = prune_fixed_joints(get_model_joints(mbp, robot))
robot_conf = Conf(robot_joints, get_configuration(mbp, context, robot))
init = [
('Robot', robot_name),
('CanMove', robot_name),
('Conf', robot_name, robot_conf),
('AtConf', robot_name, robot_conf),
('HandEmpty', robot_name),
]
goal_literals = [
('AtConf', robot_name, robot_conf),
#('Holding', robot_name, get_model_name(mbp, task.movable[0])),
]
for obj in task.movable:
obj_name = get_model_name(mbp, obj)
#obj_frame = get_base_body(mbp, obj).body_frame()
obj_pose = Pose(mbp, world, obj, get_world_pose(mbp, context, obj)) # get_relative_transform
init += [('Graspable', obj_name),
('Pose', obj_name, obj_pose),
('AtPose', obj_name, obj_pose)]
for surface in task.surfaces:
init += [('Stackable', obj_name, surface)]
#if is_placement(body, surface):
# init += [('Supported', body, pose, surface)]
for surface in task.surfaces:
surface_name = get_model_name(mbp, 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 = mbp.tree().get_body(door)
door_name = door_body.name()
door_joints = get_parent_joints(mbp, 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_open_positions(door_body)]: #, get_closed_positions(door_body)]:
conf = Conf(door_joints, positions)
init += [('Conf', door_name, conf)]
#goal_literals += [('AtConf', door_name, conf)]
goal_literals += [('AtConf', door_name, door_conf)]
for obj, surface in task.goal_on:
obj_name = get_model_name(mbp, obj)
goal_literals.append(('On', obj_name, surface))
for obj in task.goal_cooked:
obj_name = get_model_name(mbp, obj)
goal_literals.append(('Cooked', obj_name))
goal = And(*goal_literals)
print('Initial:', init)
print('Goal:', goal)
stream_map = {
'sample-pose': from_gen_fn(get_stable_gen(task, context, collisions=collisions)),
'sample-grasp': from_gen_fn(get_grasp_gen(task)),
'inverse-kinematics': from_fn(get_ik_fn(task, context, collisions=collisions)),
'plan-motion': from_fn(get_motion_fn(task, context, collisions=collisions)),
'plan-pull': from_fn(get_pull_fn(task, context, collisions=collisions)),
'TrajPoseCollision': get_collision_test(task, context, collisions=collisions),
'TrajConfCollision': get_collision_test(task, context, collisions=collisions),
}
#stream_map = 'debug'
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal