def main(args):
NOISE=0.00005
# get a bunch of random blocks
if args.use_vision:
with open(args.blocks_file, 'rb') as handle:
blocks = pickle.load(handle)[:10]
blocks = [blocks[1], blocks[2]]
else:
blocks = get_adversarial_blocks(num_blocks=args.num_blocks)
agent = PandaAgent(blocks, NOISE,
use_platform=False, teleport=False,
use_action_server=args.use_action_server,
use_vision=args.use_vision)
agent.execute()
q1 = [0.14654724763473187, 0.21575351569801834, -0.5849719978198809, -2.2556906748089887, -0.6774525616969749, 2.550307508491763, -0.6516565082767167]
q2 = [ -0.14025885624634593, 0.7700725373762062, -0.8396436988360126, -1.5055244809443935, 0.8368880648676954, 1.5269749777979318, -1.9275477043905236]
print(sum(np.abs(np.subtract(q1, q2))))
q1 = [-1.9857389225545983, -1.308321625545317, 2.0046107570246647, -2.319278960161609, 0.28307999774306025, 2.756969932860798, 2.387599795313007]
q2 = [ 1.14995860276477,-1.0715086746146851, 1.821738397538161, -2.8114018193240318, 1.9648736125191266, 1.671743112918798, -0.9022175132022459]
print(sum(np.subtract(q1, q2)))
print(sum(np.abs(np.subtract(q1, q2))))
q1 = [ -1.8355401421019457, 1.0773125105481844, 1.7073429337217094, -2.1553081323948593, -1.5797996042351563, 1.5594502235915926, -0.43448741828070747]
q2 = [-2.486464799947595, -1.0559115633903473, 1.8381697061523825, -2.6442660035967593, 2.5008998526608957, 1.5902258537646063, -1.4465391565816694]
print(sum(np.subtract(q2, q1)))
print(sum(np.abs(np.subtract(q1, q2))))
agent.execution_robot.arm.SetJointValues(q1)
input()
agent.execution_robot.arm.SetJointValues(q2)
input()
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 test_tower_simulation(blocks):
agent = PandaAgent(blocks, NOISE)
for b_ix, block in enumerate(blocks):
belief = ParticleBelief(block,
N=200,
plot=False,
vis_sim=False,
noise=NOISE)
block.com_filter = belief.particles
tp = TowerPlanner()
tallest_tower = tp.plan(blocks, num_samples=10)
# and visualize the result
agent.simulate_tower(tallest_tower, vis=True, T=2500)
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 main(args):
NOISE = 0.00005
# get a bunch of random blocks
blocks = get_adversarial_blocks(num_blocks=args.num_blocks)
if args.agent == 'teleport':
agent = TeleportAgent(blocks, NOISE)
elif args.agent == 'panda':
agent = PandaAgent(blocks, NOISE, use_platform=True, teleport=False)
else:
raise NotImplementedError()
# construct a world containing those blocks
beliefs = [
ParticleBelief(block, N=200, plot=True, vis_sim=False, noise=NOISE)
for block in blocks
]
agent._add_text('Ready?')
input('Start?')
# Gain information about the CoM of each block.
for b_ix, (block, belief) in enumerate(zip(blocks, beliefs)):
print('Running filter for', block.name)
for interaction_num in range(5):
print("Interaction number: ", interaction_num)
agent._add_text('Planning action.')
action = plan_action(belief,
exp_type='reduce_var',
action_type='place')
observation = agent.simulate_action(action, b_ix, T=50)
agent._add_text('Updating particle belief.')
belief.update(observation)
block.com_filter = belief.particles
print(belief.estimated_coms[-1], block.com)
# Find the tallest tower
print('Finding tallest tower.')
# agent._add_text('Planning tallest tower')
tp = TowerPlanner(plan_mode='expectation')
tallest_tower = tp.plan(blocks)
# and execute the resulting plan.
agent.simulate_tower(tallest_tower,
vis=True,
T=2500,
save_tower=args.save_tower)
def main(args):
NOISE = 0.00005
blocks = load_blocks(fname=args.blocks_file, num_blocks=10, remove_ixs=[1])
agent = PandaAgent(blocks,
NOISE,
use_platform=False,
teleport=False,
use_action_server=False,
use_vision=False)
# check_tower_position(agent, blocks, (0.5, -0.3))
#check_regrasp_position(agent, blocks, (0.3, 0.3))
#validate_regrasps(agent, blocks, (-0.4, -0.4))
check_initial_positions(agent, blocks)
def main(args):
NOISE=0.00005
# define real world block params and initial poses
#TODO: Load blocks from pickle file.
if args.use_vision:
with open(args.blocks_file, 'rb') as handle:
blocks = pickle.load(handle)
block_init_xy_poses = None # These will be initialized randomly but updated by the vision system.
else:
# block0 = Object('block0', Dimensions(.0381,.0318,.0635), 1.0, Position(0,0,0), Color(1,0,0))
# block1 = Object('block1', Dimensions(.0381,.0587,.0635), 1.0, Position(0,0,0), Color(0,0,1))
# block2 = Object('block2', Dimensions(.0635,.0381,.0746), 1.0, Position(0,0,0), Color(0,1,0))
block0 = Object('block0', Dimensions(.0381,.0318,.05), 1.0, Position(0,0,0), Color(1,0,0))
block1 = Object('block1', Dimensions(.0381,.0587,.06), 1.0, Position(0,0,0), Color(0,0,1))
block2 = Object('block2', Dimensions(.0635,.0381,.05), 1.0, Position(0,0,0), Color(0,1,0))
blocks = [block0, block1, block2]
block_init_xy_poses = [Pose(Position(0.65,0.3,0), Quaternion(0,0,0,1)),
Pose(Position(0.65,0.15,0), Quaternion(0,0,0,1)),
Pose(Position(0.65,0.0,0), Quaternion(0,0,0,1))]
panda = PandaAgent(blocks, NOISE,
use_platform=False,
block_init_xy_poses=block_init_xy_poses,
teleport=False,
use_vision=args.use_vision,
use_action_server=args.use_action_server,
real=args.real)
# for now hard-code a tower, but in the future will be supplied from
# active data collection or tower found through planning for evaluation
tower_blocks = copy.copy(blocks)
if args.use_vision:
tower_poses = [Pose(Position(0.5,-0.25,0.0725), Quaternion(0,0,0,1)),
Pose(Position(0.5,-0.25,0.18), Quaternion(0,0,0,1)),
Pose(Position(0.5,-0.25,0.28), Quaternion(0,0,0,1))]
else:
tower_poses = [Pose(Position(0.3,0.25,.0318), Quaternion(0,0,0,1)),
Pose(Position(0.3,0.25,.0953), Quaternion(0,0,0,1)),
Pose(Position(0.3,0.25,.1643), Quaternion(0,0,0,1))]
tower = []
for block, pose in zip(tower_blocks, tower_poses):
block.set_pose(pose)
block = get_rotated_block(block) # NOTE: have to do to set rotations field of block
tower.append(block)
# and execute the resulting plan.
if args.use_action_server:
panda.simulate_tower_parallel(tower, base_xy=(0.5, -0.25), real=args.real, vis=True, T=2500)
else:
panda.simulate_tower(tower, base_xy=(0.5, -0.25), real=args.real, vis=True, T=2500)
def main(args):
NOISE=0.00005
# get a bunch of random blocks
# if args.use_vision:
if True:
blocks = load_blocks(fname=args.blocks_file,
num_blocks=args.num_blocks)
else:
blocks = get_adversarial_blocks(num_blocks=args.num_blocks)
agent = PandaAgent(blocks, NOISE,
use_platform=False, teleport=False,
use_action_server=args.use_action_server,
use_vision=args.use_vision,
real=args.real)
if args.show_frames:
agent.step_simulation(T=1, vis_frames=True, lifeTime=0.)
input('Start building?')
p.removeAllUserDebugItems()
for tx in range(0, args.num_towers):
# Build a random tower out of blocks.
n_blocks = np.random.randint(2, args.num_blocks + 1)
tower_blocks = np.random.choice(blocks, n_blocks, replace=False)
tower = sample_random_tower(tower_blocks)
#tower = build_tower(tower_blocks, constructable=True, max_attempts=50000)
# and execute the resulting plan.
print(f"Starting tower {tx}")
if args.use_action_server:
agent.simulate_tower_parallel(tower,
real=args.real,
base_xy=(0.5, -0.3),
vis=True,
T=2500)
else:
success, stable = agent.simulate_tower(tower,
real=args.real,
base_xy=(0.5, -0.3),
vis=True,
T=2500,
save_tower=args.save_tower)
if not success:
print('Planner failed.')
print(f"Finished tower {tx}")
def main(args):
NOISE = 0.00005
# get a bunch of random blocks
# if args.use_vision:
if True:
blocks = load_blocks(args.blocks_file, 10, [1])
else:
blocks = get_adversarial_blocks(num_blocks=args.num_blocks)
input('Please make sure blocks are behind the robot.')
agent = PandaAgent(blocks,
NOISE,
use_platform=False,
teleport=False,
use_action_server=args.use_action_server,
use_vision=args.use_vision,
real=args.real)
original_poses = [b.get_base_link_pose() for b in agent.pddl_blocks]
while True:
input('Set blocks to clean up position. Ready?')
agent._update_block_poses()
# Check which blocks are in the front of the table.
agent.moved_blocks = set()
for b in agent.pddl_blocks:
pos = b.get_base_link_point()
if pos[0] > 0.05:
agent.moved_blocks.add(b)
# Clean up.
agent.plan_reset_parallel(original_poses,
real=args.real,
T=2500,
import_ros=True)
def run_active_towers(args):
logger = ActiveExperimentLogger.setup_experiment_directory(args)
# Initialize agent with supplied blocks (only works with args.block_set_fname set)
if len(args.pool_fname) > 0:
raise NotImplementedError()
elif args.block_set_fname is not '':
with open(args.block_set_fname, 'rb') as f:
block_set = pickle.load(f)
else:
raise NotImplementedError()
if args.exec_mode == 'simple-model' or args.exec_mode == 'noisy-model':
agent = None
elif args.exec_mode == 'sim' or args.exec_mode == 'real':
if args.use_panda_server:
agent = PandaClientAgent()
else:
block_set = load_blocks(fname=args.block_set_fname,
num_blocks=10,
remove_ixs=[1])
agent = PandaAgent(block_set)
# Initialize ensemble.
if args.model == 'fcgn':
base_model = FCGN
base_args = {'n_hidden': args.n_hidden, 'n_in': 14}
elif args.model == 'fcgn-fc':
base_model = FCGNFC
base_args = {'n_hidden': args.n_hidden, 'n_in': 14}
elif args.model == 'fcgn-con':
base_model = ConstructableFCGN
base_args = {'n_hidden': args.n_hidden, 'n_in': 14}
elif args.model == 'lstm':
base_model = TowerLSTM
base_args = {'n_hidden': args.n_hidden, 'n_in': 14}
elif args.model == 'bottomup-shared':
base_model = BottomUpNet
base_args = {'n_hidden': args.n_hidden, 'n_in': 14, 'share_weights': True, 'max_blocks': 5}
elif args.model == 'bottomup-unshared':
base_model = BottomUpNet
base_args = {'n_hidden': args.n_hidden, 'n_in': 14, 'share_weights': False, 'max_blocks': 5}
else:
raise NotImplementedError()
ensemble = Ensemble(base_model=base_model,
base_args=base_args,
n_models=args.n_models)
# Choose a sampler and check if we are limiting the blocks to work with.
block_set = None
if len(args.pool_fname) > 0:
pool_sampler = PoolSampler(args.pool_fname)
data_subset_fn = pool_sampler.get_subset
data_sampler_fn = pool_sampler.sample_unlabeled_data
elif args.block_set_fname is not '':
data_subset_fn = get_subset
with open(args.block_set_fname, 'rb') as f:
# TODO: Unify block loading
block_set = pickle.load(f)
if args.exec_mode == "sim" or args.exec_mode == "real":
block_set = load_blocks(fname=args.block_set_fname,
num_blocks=10)
data_sampler_fn = lambda n: sample_unlabeled_data(n, block_set=block_set)
else:
data_subset_fn = get_subset
data_sampler_fn = sample_unlabeled_data
# Sample initial dataset.
if len(args.init_data_fname) > 0:
print(f'Loading an initial dataset from {args.init_data_fname}')
# A good dataset to use is learning/data/random_blocks_(x40000)_5blocks_uniform_mass.pkl
with open(args.init_data_fname, 'rb') as handle:
towers_dict = pickle.load(handle)
dataset = TowerDataset(towers_dict,
augment=True,
K_skip=4) # From this dataset, this means we start with 10 towers/size (before augmentation).
with open('learning/data/random_blocks_(x1000.0)_constructable_val.pkl', 'rb') as handle:
val_dict = pickle.load(handle)
val_dataset = TowerDataset(val_dict,
augment=True,
K_skip=10)
elif args.sampler == 'sequential':
print('Sampling initial dataset sequentially. Dataset NOT sampled on real robot.')
towers_dict = sample_sequential_data(block_set, None, 40)
towers_dict = get_labels(towers_dict, 'noisy-model', agent, logger, args.xy_noise)
dataset = TowerDataset(towers_dict, augment=True, K_skip=1)
val_towers_dict = sample_sequential_data(block_set, None, 40)
val_towers_dict = get_labels(val_towers_dict, 'noisy-model', agent, logger, args.xy_noise)
val_dataset = TowerDataset(val_towers_dict, augment=False, K_skip=1)
if block_set is None:
raise NotImplementedError()
data_sampler_fn = lambda n_samples: sample_sequential_data(block_set, dataset, n_samples)
else:
print('Sampling initial dataset randomly.')
towers_dict = sample_unlabeled_data(40, block_set=block_set)
towers_dict = get_labels(towers_dict, args.exec_mode, agent, logger, args.xy_noise)
dataset = TowerDataset(towers_dict, augment=True, K_skip=1)
val_towers_dict = sample_unlabeled_data(40, block_set=block_set)
val_towers_dict = get_labels(val_towers_dict, args.exec_mode, agent, logger, args.xy_noise)
val_dataset = TowerDataset(val_towers_dict, augment=False, K_skip=1)
if args.strategy == 'subtower-greedy':
data_sampler_fn = lambda n_samples, bases: sample_next_block(n_samples, bases, block_set)
if args.strategy == 'subtower':
data_sampler_fn = lambda n: sample_unlabeled_data(n, block_set=block_set, range_n_blocks=(5, 5))
#print(len(dataset), len(val_dataset))
sampler = TowerSampler(dataset=dataset,
batch_size=args.batch_size,
shuffle=True,
oversample=False)
dataloader = DataLoader(dataset,
batch_sampler=sampler)
val_sampler = TowerSampler(dataset=val_dataset,
batch_size=args.batch_size,
shuffle=False)
val_dataloader = DataLoader(val_dataset,
batch_sampler=val_sampler)
print('Starting training from scratch.')
if args.exec_mode == 'real':
input('Press enter to confirm you want to start training from scratch.')
active_train(ensemble=ensemble,
dataset=dataset,
val_dataset=val_dataset,
dataloader=dataloader,
val_dataloader=val_dataloader,
data_sampler_fn=data_sampler_fn,
data_label_fn=get_labels,
data_pred_fn=get_predictions,
data_subset_fn=data_subset_fn,
logger=logger,
agent=agent,
args=args)
"""
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('--agent', choices=['teleport', 'panda'], default='teleport')
parser.add_argument('--n-particles', type=int, default=10)
parser.add_argument('--n-actions', type=int, default=2)
args = parser.parse_args()
NOISE=0.00005
# get a bunch of random blocks
blocks = get_adversarial_blocks()
if args.agent == 'teleport':
agent = TeleportAgent(blocks, NOISE)
else:
agent = PandaAgent(blocks, NOISE, teleport=False)
# construct a world containing those blocks
for b_ix, block in enumerate(blocks):
# new code
print('Running filter for', block.name, block.dimensions)
belief = ParticleBelief(block,
N=args.n_particles,
plot=True,
vis_sim=False,
noise=NOISE)
for interaction_num in range(args.n_actions):
print('----------')
# print(belief.particles.particles[::4, :])
print("Interaction number: ", interaction_num)
action = plan_action(belief, exp_type='reduce_var', action_type='place')
def main(args):
# Load the tower data
with open(args.towers_file, "rb") as f:
towers_data = pickle.load(f)
n_towers = sum([len(towers_data[k]) for k in towers_data])
print(f"Evaluating on {n_towers} towers ...")
# Check if a labeled file already exists
labeled_file = args.towers_file[:-4] + "_labeled.pkl"
if os.path.exists(labeled_file):
with open(labeled_file, "rb") as f:
labeled_data = pickle.load(f)
n_labeled = sum([len(labeled_data[k]) for k in labeled_data])
print(f"Found existing labeled file with {n_labeled} towers")
else:
print(f"Creating new file: {labeled_file}")
labeled_data = {}
for k in towers_data:
labeled_data[k] = []
n_labeled = 0
# Instantiate the tower execution agent
blocks = load_blocks(fname=args.blocks_file, num_blocks=args.num_blocks)
agent = PandaAgent(blocks,
use_platform=False,
teleport=False,
use_action_server=True,
use_vision=args.use_vision,
real=args.real)
# Loop through all the towers in the list
for k in towers_data:
for tx, data in enumerate(towers_data[k]):
# Check if tower was already labeled
if tx < n_labeled:
print(f"Already labeled tower {tx}")
continue
# Instruct the agent to build the tower
tower, max_reward, reward = data
n_blocks = len(tower)
print(f"Starting tower {tx}")
success, stable, n_stacked = agent.simulate_tower_parallel(
tower,
real=args.real,
base_xy=(0.5, -0.3),
vis=True,
T=2500,
ignore_resets=True)
# If successful, add the stability information to the list
if success:
print(
f"Finished {k} tower {tx} with stable: {stable}, num successful: {n_stacked}/{n_blocks}"
)
labeled_data[k].append(
(tower, max_reward, reward, stable, n_stacked))
with open(labeled_file, "wb") as f:
pickle.dump(labeled_data, f)
input("Reset the world and press Enter to continue.")
else:
input("Failed! Reset the world and press Enter to try again")
# Reinitialize the planning state of the world
agent.reset_world()
print("\nEvaluations complete!\n")
"""
import rospy
import argparse
from agents.panda_agent import PandaAgent
from tamp.misc import load_blocks
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument('--num-blocks', type=int, default=4)
parser.add_argument('--real', default=False, action='store_true')
parser.add_argument('--use-vision', default=False, action='store_true')
parser.add_argument(
'--blocks-file',
default='learning/domains/towers/final_block_set_10.pkl',
type=str)
args = parser.parse_args()
# Load the blocks
blocks = load_blocks(fname=args.blocks_file, num_blocks=args.num_blocks)
block_init_xy_poses = None
# Create Panda agent and leave it running as ROS node
agent = PandaAgent(blocks,
use_vision=args.use_vision,
real=args.real,
use_action_server=True,
use_learning_server=True)
print("Panda agent server ready!")
rospy.spin()
def restart_active_towers(exp_path, args):
logger = ActiveExperimentLogger.get_experiments_logger(exp_path, args)
# starting dataset (must be at least one in the exp_path)
dataset = logger.load_dataset(logger.acquisition_step)
val_dataset = logger.load_val_dataset(logger.acquisition_step)
sampler = TowerSampler(dataset=dataset,
batch_size=args.batch_size,
shuffle=True,
oversample=False)
dataloader = DataLoader(dataset,
batch_sampler=sampler)
val_sampler = TowerSampler(dataset=val_dataset,
batch_size=args.batch_size,
shuffle=False)
val_dataloader = DataLoader(val_dataset,
batch_sampler=val_sampler)
# only works with args.block_set_fname set
if args.block_set_fname is '':
raise NotImplementedError()
if args.exec_mode == 'simple-model' or args.exec_mode == 'noisy-model':
agent = None
elif args.exec_mode == 'sim' or args.exec_mode == 'real':
if args.use_panda_server:
agent = PandaClientAgent()
else:
agent = PandaAgent(block_set)
# Choose a sampler and check if we are limiting the blocks to work with.
block_set = None
if len(args.pool_fname) > 0:
pool_sampler = PoolSampler(args.pool_fname)
data_subset_fn = pool_sampler.get_subset
data_sampler_fn = pool_sampler.sample_unlabeled_data
elif args.block_set_fname is not '':
data_subset_fn = get_subset
with open(args.block_set_fname, 'rb') as f:
block_set = pickle.load(f)
if args.exec_mode == "sim" or args.exec_mode == "real":
block_set = load_blocks(fname=args.block_set_fname,
num_blocks=10)
data_sampler_fn = lambda n: sample_unlabeled_data(n, block_set=block_set)
else:
data_subset_fn = get_subset
data_sampler_fn = sample_unlabeled_data
if args.sampler == 'sequential':
data_sampler_fn = lambda n_samples: sample_sequential_data(block_set, dataset, n_samples)
if args.strategy == 'subtower-greedy':
data_sampler_fn = lambda n_samples, bases: sample_next_block(n_samples, bases, block_set)
if args.strategy == 'subtower':
data_sampler_fn = lambda n: sample_unlabeled_data(n, block_set=block_set, range_n_blocks=(5, 5))
print("Setting up dataset")
ensemble = setup_active_train(dataset,
val_dataset,
dataloader=dataloader,
val_dataloader=val_dataloader,
logger=logger,
data_sampler_fn=data_sampler_fn,
data_label_fn=get_labels,
data_pred_fn=get_predictions,
data_subset_fn=data_subset_fn,
agent=agent,
args=args)
print("Restarting active learning")
active_train(ensemble=ensemble,
dataset=dataset,
val_dataset=val_dataset,
dataloader=dataloader,
val_dataloader=val_dataloader,
data_sampler_fn=data_sampler_fn,
data_label_fn=get_labels,
data_pred_fn=get_predictions,
data_subset_fn=data_subset_fn,
logger=logger,
agent=agent,
args=args)
def main(args):
NOISE = 0.00005
# get a bunch of random blocks
blocks = load_blocks(fname=args.blocks_file, num_blocks=10, remove_ixs=[1])
agent = PandaAgent(blocks,
NOISE,
use_platform=False,
teleport=False,
use_action_server=False,
use_vision=False,
real=True)
agent.step_simulation(T=1, vis_frames=True, lifeTime=0.)
agent.plan()
fixed = [f for f in agent.fixed if f is not None]
grasps_fn = get_grasp_gen(agent.robot,
add_slanted_grasps=False,
add_orthogonal_grasps=True)
path_planner = get_free_motion_gen(agent.robot, fixed)
ik_fn = get_ik_fn(agent.robot,
fixed,
approach_frame='gripper',
backoff_frame='global',
use_wrist_camera=False)
from franka_interface import ArmInterface
arm = ArmInterface()
#arm.move_to_neutral()
start_q = arm.convertToList(arm.joint_angles())
start_q = pb_robot.vobj.BodyConf(agent.robot, start_q)
body = agent.pddl_blocks[args.id]
pose = pb_robot.vobj.BodyPose(body, body.get_base_link_pose())
for g in grasps_fn(body):
grasp = g[0]
# Check that the grasp points straight down.
obj_worldF = pb_robot.geometry.tform_from_pose(pose.pose)
grasp_worldF = np.dot(obj_worldF, grasp.grasp_objF)
grasp_worldR = grasp_worldF[:3, :3]
e_x, e_y, e_z = np.eye(3)
is_top_grasp = grasp_worldR[:, 2].dot(-e_z) > 0.999
if not is_top_grasp: continue
print('Getting IK...')
approach_q = ik_fn(body, pose, grasp, return_grasp_q=True)[0]
print('Planning move to path')
command1 = path_planner(start_q, approach_q)
print('Planning return home')
command2 = path_planner(approach_q, start_q)
agent.execute()
input('Ready to execute?')
command1[0][0].simulate(timestep=0.25)
input('Move back in sim?')
command2[0][0].simulate(timestep=0.25)
input('Move to position on real robot?')
command1[0][0].execute(realRobot=arm)
input('Reset position on real robot?')
command2[0][0].execute(realRobot=arm)
break