def test_placement_ik(agent, blocks):
"""
To make sure that the platform is in a good position, make sure the
IK is feasible for some grasp position.
"""
get_block_pose = tamp.primitives.get_stable_gen_block(
[agent.table, agent.platform])
get_grasp = tamp.primitives.get_grasp_gen(agent.robot)
get_ik = tamp.primitives.get_ik_fn(agent.robot,
[agent.platform, agent.table])
for r in list(rotation_group()):
r = list(rotation_group())[4]
action = PlaceAction(pos=None, rot=r, block=blocks[0])
blocks[0].set_pose(Pose(ZERO_POS, Quaternion(*action.rot.as_quat())))
rotated_block = get_rotated_block(blocks[0])
x = action.pos[0]
y = action.pos[1]
z = agent.platform.get_dimensions(
)[2] / 2 + rotated_block.dimensions[2] / 2 + 1e-5
tform = numpy.array([[1., 0., 0., x], [0., 1., 0., y], [0., 0., 1., z],
[0., 0., 0., 1.]])
tform[0:3, 0:3] = action.rot.as_matrix()
platform_pose = pb_robot.vobj.BodyPose(
agent.platform, agent.platform.get_base_link_pose())
start_pose = pb_robot.vobj.BodyPose(
agent.pddl_blocks[0], agent.pddl_blocks[0].get_base_link_pose())
placement_pose = get_block_pose(agent.pddl_blocks[0], agent.platform,
platform_pose, tform)[0]
ik_found = False
for grasp in get_grasp(agent.pddl_blocks[0]):
ik_start = get_ik(agent.pddl_blocks[0], start_pose, grasp[0])
ik_placement = get_ik(agent.pddl_blocks[0], placement_pose,
grasp[0])
if ik_start is not None:
print('Y', end='')
else:
print('N', end='')
if ik_placement is not None:
ik_found = True
print('Y', end=' ')
else:
print('N', end=' ')
if ik_found:
print('Found IK.')
else:
print('No IK.')
break
def test_return_to_start(blocks, n_placements=5, rot_ix=0, block_ix=1):
"""
Let a block fall off the platform and check that we can successfully
pick it up and return it to the starting position.
"""
numpy.random.seed(10)
rot = list(rotation_group())[rot_ix]
for _ in range(n_placements):
# Create agent.
agent = PandaAgent(blocks)
original_pose = agent.pddl_blocks[block_ix].get_base_link_pose()
# Create a random action.
new_dims = numpy.abs(rot.apply(blocks[block_ix].dimensions))
place_pos = new_dims * (-0.5 * numpy.random.rand(3))
x, y, _ = place_pos + numpy.array(agent.platform.get_dimensions()) / 2
action = PlaceAction(pos=None, rot=rot, block=blocks[block_ix])
# Teleport block to platform.
blocks[block_ix].set_pose(
Pose(ZERO_POS, Quaternion(*action.rot.as_quat())))
rotated_block = get_rotated_block(blocks[block_ix])
platform_pose = agent.platform.get_base_link_pose()
platform_tform = pb_robot.geometry.tform_from_pose(platform_pose)
z = agent.platform.get_dimensions(
)[2] / 2 + rotated_block.dimensions[2] / 2 + 1e-5
tform = numpy.array([[1., 0., 0., action.pos[0]],
[0., 1., 0., action.pos[1]], [0., 0., 1., z],
[0., 0., 0., 1.]])
tform[0:3, 0:3] = action.rot.as_matrix()
body_tform = platform_tform @ tform
pose = pb_robot.geometry.pose_from_tform(body_tform)
agent.pddl_blocks[block_ix].set_base_link_pose(pose)
# Execute action.
p.setGravity(0, 0, -10)
for _ in range(500):
p.stepSimulation()
time.sleep(0.01)
check_ungraspable_block(agent)
# Solve PDDL Problem.
pddl_block = agent.pddl_blocks[block_ix]
init = agent._get_initial_pddl_state()
goal_pose = pb_robot.vobj.BodyPose(pddl_block, original_pose)
init += [('Pose', pddl_block, goal_pose),
('Supported', pddl_block, goal_pose, agent.table,
agent.table_pose)]
goal = ('and', ('AtPose', pddl_block, goal_pose), ('On', pddl_block,
agent.table))
# Solve the PDDLStream problem.
print('Init:', init)
print('Goal:', goal)
agent._solve_and_execute_pddl(init, goal)
p.disconnect()
def test_place_action(blocks, block_ix):
"""
Test method to try placing the given blocks on the platform.
"""
agent = PandaAgent(blocks, NOISE, teleport=False)
for r in list(rotation_group())[:]:
action = PlaceAction(pos=None, rot=r, block=blocks[block_ix])
agent.simulate_action(action, block_ix)
def visualize_grasps(agent, blocks):
"""
Test method to visualize the grasps. Helps check for potential problems
due to collisions with table or underspecified grasp set.
"""
get_block_pose = tamp.primitives.get_stable_gen_table(
[agent.table, agent.platform_leg, agent.platform_table])
get_grasp = tamp.primitives.get_grasp_gen(agent.robot)
get_ik = tamp.primitives.get_ik_fn(
agent.robot, [agent.platform_leg, agent.platform_table, agent.table])
block_ix = 3
for r in list(rotation_group())[5:]:
table_pose = pb_robot.vobj.BodyPose(agent.table,
agent.table.get_base_link_pose())
pose = agent.pddl_blocks[block_ix].get_base_link_pose()
pose = ((pose[0][0] - 0.1, pose[0][1], pose[0][2] + 0.2), pose[1])
start_pose = pb_robot.vobj.BodyPose(agent.pddl_blocks[block_ix], pose)
agent.execute()
agent.pddl_blocks[block_ix].set_base_link_pose(pose)
agent.plan()
agent.pddl_blocks[block_ix].set_base_link_pose(pose)
ix = 0
for grasp in list(get_grasp(agent.pddl_blocks[block_ix])):
ik_start = get_ik(agent.pddl_blocks[block_ix], start_pose,
grasp[0])
import time
if ik_start is not None:
print(ix, 'Y')
agent.execute()
agent.robot.arm.SetJointValues(ik_start[1][0].path[-1])
agent.plan()
agent.robot.arm.SetJointValues(ik_start[1][0].path[-1])
import time
time.sleep(2)
else:
print(ix, 'N')
ix += 1
def test_observations(blocks, block_ix):
"""
Test method to try placing the given blocks on the platform.
"""
agent = PandaAgent(blocks, NOISE)
for r in list(rotation_group())[0:]:
action = PlaceAction(pos=None, rot=r, block=blocks[block_ix])
obs = agent.simulate_action(action, block_ix, T=50)
# TODO: Check that the simulated observation agrees with the true observation.
particle_block = deepcopy(blocks[block_ix])
world = make_platform_world(particle_block, action)
env = Environment([world], vis_sim=False)
if False:
env.step(action=action)
length, lifeTime = 0.2, 0.0
pos = end_pose = world.get_pose(world.objects[1])[0]
quat = action.rot.as_quat()
new_x = transformation([length, 0.0, 0.0], pos, quat)
new_y = transformation([0.0, length, 0.0], pos, quat)
new_z = transformation([0.0, 0.0, length], pos, quat)
p.addUserDebugLine(pos, new_x, [1, 0, 0], lifeTime=lifeTime)
p.addUserDebugLine(pos, new_y, [0, 1, 0], lifeTime=lifeTime)
p.addUserDebugLine(pos, new_z, [0, 0, 1], lifeTime=lifeTime)
input('Continue')
for _ in range(50):
env.step(action=action)
end_pose = world.get_pose(world.objects[1])
print('Simulated Pose:', end_pose)
print('TAMP Pose:', obs[2])
input('Continue?')
def filter_block(p_true_block, exp_type, args):
if args.plot:
plt.ion()
fig = plt.figure()
ax = Axes3D(fig)
com_particle_dist = None
experience = []
estimated_coms = []
for i in range(I):
print('Explore action %d/%d' % (i, I))
true_block = copy.deepcopy(p_true_block)
if args.plot: setup_ax(ax, true_world.objects[1])
# create particle worlds for obj_b's COM
com_ranges = get_com_ranges(true_block)
if com_particle_dist is None:
com_particle_dist = create_uniform_particles(N, D, com_ranges)
else:
# update the distribution with the new weights
print(weights)
com_particle_dist = ParticleDistribution(
com_particle_dist.particles, weights)
# and resample the distribution
com_particle_dist = sample_and_wiggle(com_particle_dist,
experience, OBS_MODEL_COV,
true_block, com_ranges)
weights = com_particle_dist.weights
particle_blocks = [
copy.deepcopy(true_block)
for particle in com_particle_dist.particles
]
for (com, particle_block) in zip(com_particle_dist.particles,
particle_blocks):
particle_block.com = com
# Choose action to maximize variance of particles.
if exp_type == 'reduce_var':
rot, direc = plan_action(particle_blocks)
elif exp_type == 'random':
rot = random.choice(list(rotation_group()))
direc = PushAction.get_random_dir()
true_world = make_platform_world(true_block, rot)
particle_worlds = [
make_platform_world(pb, rot) for pb in particle_blocks
]
env = Environment([true_world] + particle_worlds, vis_sim=args.vis)
# action to apply to all worlds
action = PushAction(block_pos=true_world.get_pose(
true_world.objects[1]).pos,
direction=direc,
timesteps=T)
for t in range(T):
env.step(action=action)
# get ground truth object_b pose (observation)
objb_pose = true_world.get_pose(true_world.objects[1])
objb_pose = add_noise(objb_pose)
experience.append((action, rot, T, objb_pose))
# update all particle weights
new_weights = []
for pi, (particle_world,
old_weight) in enumerate(zip(particle_worlds, weights)):
particle_objb_pose = particle_world.get_pose(
particle_world.objects[1])
obs_model = multivariate_normal.pdf(objb_pose.pos,
mean=particle_objb_pose.pos,
cov=OBS_MODEL_COV)
new_weight = old_weight * obs_model
new_weights.append(new_weight)
# normalize particle weights
weights_sum = sum(new_weights)
weights = np.divide(new_weights, weights_sum)
# print('max particle weight: ', max(weights))
if args.plot and not t % 5:
# visualize particles (it's very slow)
plot_particles(ax, com_particle_dist.particles, weights, t=t)
com = np.array(com_particle_dist.particles).T
print(
'Mean COM', com @ weights,
np.diag(
np.cov(com_particle_dist.particles,
rowvar=False,
aweights=weights + 1e-3)))
print('True COM', true_block.com)
print('Error COM', np.linalg.norm(true_block.com - com @ weights))
estimated_coms.append(com @ weights)
env.disconnect()
env.cleanup()
return ParticleDistribution(com_particle_dist.particles,
weights), estimated_coms
def plan_action(belief, k=1, exp_type='reduce_var', action_type='push'):
""" Given a set of particles, choose the action that maximizes the observed variance.
:param particle_block: A list of the current set of particles instantiated as blocks.
:param k: Number of pushes to do for each orientation.
"""
if action_type == 'push':
if exp_type == 'reduce_var':
print('Finding variance reducing push action')
results = []
for rot in rotation_group():
for _ in range(k):
action = PushAction(rot=rot, timesteps=50, block=belief.block)
# create a bunch of blocks with the same geometry where each COM
# for each block is set to one of the particles
particle_blocks = [copy.deepcopy(belief.block) for particle in belief.particles.particles]
for (com, particle_block) in zip(belief.particles.particles, particle_blocks):
particle_block.com = com
particle_worlds = [make_platform_world(pb, action) for pb in particle_blocks]
env = Environment(particle_worlds, vis_sim=False)
# get the the position of the block to set the start position of the push action
# action.set_push_start_pos(particle_worlds[0].get_pose(particle_worlds[0].objects[1]).pos)
for _ in range(50):
env.step(action=action)
# Get end pose of all particle blocks.
poses = np.array([w.get_pose(w.objects[1]).pos for w in particle_worlds])
var = np.var(poses, axis=0)
score = np.mean(var)
print(var, score)
results.append((action, score))
env.disconnect()
env.cleanup()
return max(results, key=itemgetter(1))[0]
else:
print('Finding random push action')
rs = [r for r in rotation_group()]
ix = np.random.choice(np.arange(len(rs)))
rot = rs[ix]
push_action = PushAction(rot=rot, block=belief.block)
return push_action
else:
if exp_type == 'reduce_var':
print('Finding variance reducing place action')
results = []
for rot in rotation_group():
for _ in range(k):
action = PlaceAction(rot=rot, pos=None, block=belief.block)
# create a bunch of blocks with the same geometry where each COM
# for each block is set to one of the particles
particle_blocks = [copy.deepcopy(belief.block) for particle in belief.particles.particles]
for (com, particle_block) in zip(belief.particles.particles, particle_blocks):
particle_block.com = com
particle_worlds = [make_platform_world(pb, action) for pb in particle_blocks]
env = Environment(particle_worlds, vis_sim=False)
# get the the position of the block to set the start position of the push action
# action.set_push_start_pos(particle_worlds[0].get_pose(particle_worlds[0].objects[1]).pos)
for _ in range(50):
env.step(action=action)
# Get end pose of all particle blocks.
poses = np.array([w.get_pose(w.objects[1]).pos for w in particle_worlds])
var = np.var(poses, axis=0)
score = np.max(var)
cov = np.cov(poses, rowvar=False)
w, v = np.linalg.eig(cov)
#print(w)
score = np.max(w)
#print(var, score)
results.append((action, score))
env.disconnect()
env.cleanup()
return max(results, key=itemgetter(1))[0]
else:
print('Finding random place action')
# pick a random rotation
rs = [r for r in rotation_group()]
ix = np.random.choice(np.arange(len(rs)))
rot = rs[ix]
# construct the corresponding place action
place_action = PlaceAction(rot=rot, pos=None, block=belief.block)
return place_action
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('--action-type', choices=['push', 'place'], required=True)
parser.add_argument('--agent-type', choices=['teleport', 'panda'], required=True)
args = parser.parse_args()
# Create the block.
true_com = Position(x=0., y=0., z=0.)
block = Object(name='block',
dimensions=Dimensions(x=0.05, y=0.1, z=0.05),
mass=1,
com=true_com,
color=Color(r=1., g=0., b=0.))
# Test the action for all rotations.
for r in rotation_group():
# Create the action.
if args.action_type == 'push':
action = PushAction(direction=None,
timesteps=50,
rot=r,
block=block)
elif args.action_type == 'place':
action = PlaceAction(pos=None,
rot=r)
else:
raise NotImplementedError()
# Make worlds for each block and test using different CoMs.
true_world = make_platform_world(block, action)
com_ranges = get_com_ranges(true_world.objects[1])
