def main():
# define simulation objects
table_height = 0.77
cyl_radius = 0.025
cyl_height = 0.3
cyl_pos0 = np.r_[0.7, -0.15, table_height + cyl_height / 2]
cyl_pos1 = np.r_[0.7, 0.15, table_height + cyl_height / 2]
cyl_pos2 = np.r_[0.4, -0.15, table_height + cyl_height / 2]
rope_poss = np.array(
[
[0.2, -0.2, table_height + 0.006],
[0.8, -0.2, table_height + 0.006],
[0.8, 0.2, table_height + 0.006],
[0.2, 0.2, table_height + 0.006],
]
)
sim_objs = []
sim_objs.append(XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False))
sim_objs.append(BoxSimulationObject("table", [1, 0, table_height - 0.1], [0.85, 0.85, 0.1], dynamic=False))
cyl_sim_objs = create_cylinder_grid(cyl_pos0, cyl_pos1, cyl_pos2, cyl_radius, cyl_height)
sim_objs.extend(cyl_sim_objs)
rope_sim_obj = create_rope(rope_poss)
sim_objs.append(rope_sim_obj)
# initialize simulation world and environment
sim = DynamicSimulationRobotWorld()
sim.add_objects(sim_objs)
sim.create_viewer()
sim.robot.SetDOFValues([0.25], [sim.robot.GetJoint("torso_lift_joint").GetJointIndex()])
sim_util.reset_arms_to_side(sim)
color_cylinders(cyl_sim_objs)
env = environment.LfdEnvironment(sim, sim)
# define augmented trajectory
pick_pos = rope_poss[0] + 0.1 * (rope_poss[1] - rope_poss[0])
drop_pos = rope_poss[3] + 0.1 * (rope_poss[2] - rope_poss[3]) + np.r_[0, 0.2, 0]
pick_R = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])
drop_R = np.array([[0, 1, 0], [0, 0, -1], [-1, 0, 0]])
move_height = 0.2
aug_traj = create_augmented_traj(sim.robot, pick_pos, drop_pos, pick_R, drop_R, move_height)
env.execute_augmented_trajectory(aug_traj)
def replay_on_holdout(args, action_selection, reg_and_traj_transferer, lfd_env, sim):
loadresultfile = h5py.File(args.replay.loadresultfile, 'r')
loadresult_items = eval_util.get_indexed_items(loadresultfile, task_list=args.tasks, task_file=args.taskfile, i_start=args.i_start, i_end=args.i_end)
num_successes = 0
num_total = 0
for i_task, task_info in loadresult_items:
redprint("task %s" % i_task)
for i_step in range(len(task_info)):
redprint("task %s step %i" % (i_task, i_step))
replay_results = eval_util.load_task_results_step(args.replay.loadresultfile, i_task, i_step)
sim_state = replay_results['sim_state']
if i_step > 0: # sanity check for reproducibility
sim_util.reset_arms_to_side(sim)
if sim.simulation_state_equal(sim_state, sim.get_state()):
yellowprint("Reproducible results OK")
else:
yellowprint("The replayed simulation state doesn't match the one from the result file")
sim.set_state(sim_state)
if args.replay.simulate_traj_steps is not None and i_step not in args.replay.simulate_traj_steps:
continue
if i_step in args.replay.compute_traj_steps: # compute the trajectory in this step
best_root_action = replay_results['best_action']
scene_state = replay_results['scene_state']
# plot cloud of the test scene
handles = []
if args.plotting:
handles.append(sim.env.plot3(scene_state.cloud[:,:3], 2, scene_state.color if scene_state.color is not None else (0,0,1)))
sim.viewer.Step()
test_aug_traj = reg_and_traj_transferer.transfer(GlobalVars.demos[best_root_action], scene_state, plotting=args.plotting)
else:
test_aug_traj = replay_results['aug_traj']
feasible, misgrasp = lfd_env.execute_augmented_trajectory(test_aug_traj, step_viewer=args.animation, interactive=args.interactive, check_feasible=args.eval.check_feasible)
if replay_results['knot']:
num_successes += 1
num_total += 1
redprint('REPLAY Successes / Total: ' + str(num_successes) + '/' + str(num_total))
def main():
# define simulation objects
table_height = 0.77
sim_objs = []
sim_objs.append(XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False))
sim_objs.append(BoxSimulationObject("table", [1, 0, table_height-.1], [.85, .85, .1], dynamic=False))
# initialize simulation world and environment
sim = DynamicSimulationRobotWorld()
sim.add_objects(sim_objs)
sim.create_viewer()
sim.robot.SetDOFValues([0.25], [sim.robot.GetJoint('torso_lift_joint').GetJointIndex()])
sim.robot.SetDOFValues([1.25], [sim.robot.GetJoint('head_tilt_joint').GetJointIndex()]) # move head down so it can see the rope
sim_util.reset_arms_to_side(sim)
env = environment.LfdEnvironment(sim, sim, downsample_size=0.025)
demo_rope_poss = np.array([[.2, -.2, table_height+0.006],
[.8, -.2, table_height+0.006],
[.8, .2, table_height+0.006],
[.2, .2, table_height+0.006]])
demo = create_rope_demo(env, demo_rope_poss)
test_rope_poss = np.array([[.2, -.2, table_height+0.006],
[.5, -.4, table_height+0.006],
[.8, .0, table_height+0.006],
[.8, .2, table_height+0.006],
[.6, .0, table_height+0.006],
[.4, .2, table_height+0.006],
[.2, .2, table_height+0.006]])
test_rope_sim_obj = create_rope(test_rope_poss)
sim.add_objects([test_rope_sim_obj])
sim.settle()
test_scene_state = env.observe_scene()
reg_factory = TpsRpmRegistrationFactory()
traj_transferer = FingerTrajectoryTransferer(sim)
plot_cb = lambda i, i_em, x_nd, y_md, xtarg_nd, wt_n, f, corr_nm, rad: registration_plot_cb(sim, x_nd, y_md, f)
reg_and_traj_transferer = TwoStepRegistrationAndTrajectoryTransferer(reg_factory, traj_transferer)
test_aug_traj = reg_and_traj_transferer.transfer(demo, test_scene_state, callback=plot_cb, plotting=True)
env.execute_augmented_trajectory(test_aug_traj)
def main():
# define simulation objects
table_height = 0.77
cyl_radius = 0.025
cyl_height = 0.3
cyl_pos0 = np.r_[.7, -.15, table_height+cyl_height/2]
cyl_pos1 = np.r_[.7, .15, table_height+cyl_height/2]
cyl_pos2 = np.r_[.4, -.15, table_height+cyl_height/2]
rope_poss = np.array([[.2, -.2, table_height+0.006],
[.8, -.2, table_height+0.006],
[.8, .2, table_height+0.006],
[.2, .2, table_height+0.006]])
sim_objs = []
sim_objs.append(XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False))
sim_objs.append(BoxSimulationObject("table", [1, 0, table_height-.1], [.85, .85, .1], dynamic=False))
cyl_sim_objs = create_cylinder_grid(cyl_pos0, cyl_pos1, cyl_pos2, cyl_radius, cyl_height)
sim_objs.extend(cyl_sim_objs)
rope_sim_obj = create_rope(rope_poss)
sim_objs.append(rope_sim_obj)
# initialize simulation world and environment
sim = DynamicSimulationRobotWorld()
sim.add_objects(sim_objs)
sim.create_viewer()
sim.robot.SetDOFValues([0.25], [sim.robot.GetJoint('torso_lift_joint').GetJointIndex()])
sim_util.reset_arms_to_side(sim)
color_cylinders(cyl_sim_objs)
env = environment.LfdEnvironment(sim, sim)
# define augmented trajectory
pick_pos = rope_poss[0] + .1 * (rope_poss[1] - rope_poss[0])
drop_pos = rope_poss[3] + .1 * (rope_poss[2] - rope_poss[3]) + np.r_[0, .2, 0]
pick_R = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])
drop_R = np.array([[0, 1, 0], [0, 0, -1], [-1, 0, 0]])
move_height = .2
aug_traj = create_augmented_traj(sim.robot, pick_pos, drop_pos, pick_R, drop_R, move_height)
env.execute_augmented_trajectory(aug_traj)
def setUp(self):
table_height = 0.77
helix_ang0 = 0
helix_ang1 = 4*np.pi
helix_radius = .2
helix_center = np.r_[.6, 0]
helix_height0 = table_height + .15
helix_height1 = table_height + .15 + .3
helix_length = np.linalg.norm(np.r_[(helix_ang1 - helix_ang0) * helix_radius, helix_height1 - helix_height0])
num = np.round(helix_length/.02)
helix_angs = np.linspace(helix_ang0, helix_ang1, num)
helix_heights = np.linspace(helix_height0, helix_height1, num)
init_rope_nodes = np.c_[helix_center + helix_radius * np.c_[np.cos(helix_angs), np.sin(helix_angs)], helix_heights]
rope_params = sim_util.RopeParams()
cyl_radius = 0.025
cyl_height = 0.3
cyl_pos0 = np.r_[.6, helix_radius, table_height + .25]
cyl_pos1 = np.r_[.6, -helix_radius, table_height + .35]
sim_objs = []
sim_objs.append(BoxSimulationObject("table", [1, 0, table_height-.1], [.85, .85, .1], dynamic=False))
sim_objs.append(RopeSimulationObject("rope", init_rope_nodes, rope_params))
sim_objs.append(CylinderSimulationObject("cyl0", cyl_pos0, cyl_radius, cyl_height, dynamic=True))
sim_objs.append(CylinderSimulationObject("cyl1", cyl_pos1, cyl_radius, cyl_height, dynamic=True))
self.sim = DynamicSimulation()
self.sim.add_objects(sim_objs)
# rotate cylinders by 90 deg
for i in range(2):
bt_cyl = self.sim.bt_env.GetObjectByName('cyl%d' % i)
T = openravepy.matrixFromAxisAngle(np.array([np.pi/2, 0, 0]))
T[:3,3] = bt_cyl.GetTransform()[:3, 3]
bt_cyl.SetTransform(T) # SetTransform needs to be used in the Bullet object, not the openrave body
self.sim.update()
self.sim.add_objects([XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False)])
self.sim.robot.SetDOFValues([0.25], [self.sim.robot.GetJoint('torso_lift_joint').GetJointIndex()])
sim_util.reset_arms_to_side(self.sim)
rope_params = sim_util.RopeParams()
sim_objs = []
sim_objs.append(XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False))
sim_objs.append(BoxSimulationObject("table", [1, 0, table_height-.1], [.85, .85, .1], dynamic=False))
# add grid of cylinders
cyl_sim_objs = []
for (i,cyl_pos) in enumerate(cyl_positions):
cyl_sim_objs.append(CylinderSimulationObject("cyl%i"%i, cyl_pos, cyl_radius, cyl_height, dynamic=True))
sim_objs.extend(cyl_sim_objs)
sim_objs.append(RopeSimulationObject("rope", init_rope_nodes, rope_params))
sim = DynamicSimulationRobotWorld()
sim.add_objects(sim_objs)
sim.robot.SetDOFValues([0.25], [sim.robot.GetJoint('torso_lift_joint').GetJointIndex()])
sim_util.reset_arms_to_side(sim)
# set random colors to cylinders
for sim_obj in cyl_sim_objs:
color = np.random.random(3)
for bt_obj in sim_obj.get_bullet_objects():
for link in bt_obj.GetKinBody().GetLinks():
for geom in link.GetGeometries():
geom.SetDiffuseColor(color)
viewer = trajoptpy.GetViewer(sim.env)
camera_matrix = np.array([[ 0, 1, 0, 0],
[-1, 0, 0.5, 0],
[ 0.5, 0, 1, 0],
[ 2.25, 0, 4.5, 1]])
viewer.SetWindowProp(0,0,1500,1500)
def label_demos_parallel(args, transferer, lfd_env, sim):
outfile = h5py.File(args.eval.outfile, 'a')
rope_params = sim_util.RopeParams()
rope_params.radius = settings.ROPE_RADIUS_THICK
if args.eval.rope_param_radius is not None:
rope_params.radius = args.eval.rope_param_radius
if args.eval.rope_param_angStiffness is not None:
rope_params.angStiffness = args.eval.rope_param_angStiffness
# TODO pass in rope params to sample_rope_state
(init_rope_nodes, demo_id) = sample_rope_state(
args.eval, sim, args.animation)
resample = False
labeled_data = []
while True:
if args.animation:
sim.viewer.Step()
sim_state = sim.get_state()
sim.set_state(sim_state)
scene_state = lfd_env.observe_scene()
# plot cloud of the test scene
handles = []
if args.plotting:
handles.append(
sim.env.plot3(
scene_state.cloud[
:,
:3],
2,
scene_state.color if scene_state.color is not None else (
0,
0,
1)))
sim.viewer.Step()
costs = transferer.registration_factory.batch_cost(scene_state)
best_keys = sorted(costs, key=costs.get)
for seg_name in best_keys:
traj = transferer.transfer(GlobalVars.demos[seg_name],
scene_state,
plotting=args.plotting)
feasible, misgrasp = lfd_env.execute_augmented_trajectory(
traj, step_viewer=args.animation, interactive=args.interactive)
sim_util.reset_arms_to_side(sim)
sim.settle(step_viewer=args.animation)
if not feasible or misgrasp:
print 'Feasible: ', feasible
print 'Misgrasp: ', misgrasp
if misgrasp:
sim.set_state(sim_state)
continue
print "y accepts this action"
print "n rejects this action"
print "r resamples rope state"
print "f to save this as a failure"
print "C-c safely quits"
user_input = lower(raw_input("What to do?"))
success = False
if user_input == 'y':
success = True
labeled_data.append((scene_state,seg_name))
if isFig8Knot(get_rope_nodes(sim)):
save_success(outfile, labeled_data)
labeled_data = []
sim.set_state(sample_rope_state(args.eval, sim))
break
elif user_input == 'n':
sim.set_state(sim_state)
continue
elif user_input == 'r':
break
elif user_input == 'f':
sim.set_state(sim_state)
save_failure(outfile, lfd_env.observe_scene())
continue
if not success:
labeled_data = []
sim.set_state(sample_rope_state(args.eval, sim))
def setup_lfd_environment_sim(args):
actions = h5py.File(args.eval.actionfile, 'r')
init_rope_xyz, init_joint_names, init_joint_values = sim_util.load_fake_data_segment(
actions, args.eval.fake_data_segment, args.eval.fake_data_transform)
table_height = init_rope_xyz[:, 2].mean() - .02
sim_objs = []
sim_objs.append(
XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False))
sim_objs.append(BoxSimulationObject(
"table", [1, 0, table_height + (-.1 + .01)], [.85, .85, .1], dynamic=False))
print 'Setting up lfd environment'
sim = DynamicRopeSimulationRobotWorld()
world = sim
sim.add_objects(sim_objs)
if args.eval.ground_truth:
lfd_env = GroundTruthRopeLfdEnvironment(
sim,
world,
upsample=args.eval.upsample,
upsample_rad=args.eval.upsample_rad,
downsample_size=args.eval.downsample_size)
else:
lfd_env = LfdEnvironment(
sim, world, downsample_size=args.eval.downsample_size)
dof_inds = sim_util.dof_inds_from_name(
sim.robot, '+'.join(init_joint_names))
values, dof_inds = zip(
*[(value, dof_ind) for value, dof_ind in zip(init_joint_values, dof_inds) if dof_ind != -1])
# this also sets the torso (torso_lift_joint) to the height in the data
sim.robot.SetDOFValues(values, dof_inds)
sim_util.reset_arms_to_side(sim)
if args.animation:
viewer = trajoptpy.GetViewer(sim.env)
if os.path.isfile(args.window_prop_file) and os.path.isfile(
args.camera_matrix_file):
print "loading window and camera properties"
window_prop = np.loadtxt(args.window_prop_file)
camera_matrix = np.loadtxt(args.camera_matrix_file)
try:
viewer.SetWindowProp(*window_prop)
viewer.SetCameraManipulatorMatrix(camera_matrix)
except:
print "SetWindowProp and SetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."
else:
print "move viewer to viewpoint that isn't stupid"
print "then hit 'p' to continue"
viewer.Idle()
print "saving window and camera properties"
try:
window_prop = viewer.GetWindowProp()
camera_matrix = viewer.GetCameraManipulatorMatrix()
np.savetxt(args.window_prop_file, window_prop, fmt='%d')
np.savetxt(args.camera_matrix_file, camera_matrix)
except:
print "GetWindowProp and GetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."
viewer.Step()
if args.eval.dof_limits_factor != 1.0:
assert 0 < args.eval.dof_limits_factor and args.eval.dof_limits_factor <= 1.0
active_dof_indices = sim.robot.GetActiveDOFIndices()
active_dof_limits = sim.robot.GetActiveDOFLimits()
for lr in 'lr':
manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
dof_inds = sim.robot.GetManipulator(manip_name).GetArmIndices()
limits = np.asarray(sim.robot.GetDOFLimits(dof_inds))
limits_mean = limits.mean(axis=0)
limits_width = np.diff(limits, axis=0)
new_limits = limits_mean + args.eval.dof_limits_factor * \
np.r_[-limits_width / 2.0, limits_width / 2.0]
for i, ind in enumerate(dof_inds):
active_dof_limits[0][
active_dof_indices.tolist().index(ind)] = new_limits[0, i]
active_dof_limits[1][
active_dof_indices.tolist().index(ind)] = new_limits[1, i]
sim.robot.SetDOFLimits(active_dof_limits[0], active_dof_limits[1])
return lfd_env, sim
def setup_lfd_environment_sim(args):
actions = h5py.File(args.eval.actionfile, 'r')
init_rope_xyz, init_joint_names, init_joint_values = sim_util.load_fake_data_segment(
actions, args.eval.fake_data_segment, args.eval.fake_data_transform)
table_height = init_rope_xyz[:, 2].mean() - .02
sim_objs = []
sim_objs.append(
XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False))
sim_objs.append(
BoxSimulationObject("table", [1, 0, table_height + (-.1 + .01)],
[.85, .85, .1],
dynamic=False))
print 'Setting up lfd environment'
sim = DynamicRopeSimulationRobotWorld()
world = sim
sim.add_objects(sim_objs)
if args.eval.ground_truth:
lfd_env = GroundTruthRopeLfdEnvironment(
sim,
world,
upsample=args.eval.upsample,
upsample_rad=args.eval.upsample_rad,
downsample_size=args.eval.downsample_size)
else:
lfd_env = LfdEnvironment(sim,
world,
downsample_size=args.eval.downsample_size)
dof_inds = sim_util.dof_inds_from_name(sim.robot,
'+'.join(init_joint_names))
values, dof_inds = zip(
*[(value, dof_ind)
for value, dof_ind in zip(init_joint_values, dof_inds)
if dof_ind != -1])
# this also sets the torso (torso_lift_joint) to the height in the data
sim.robot.SetDOFValues(values, dof_inds)
sim_util.reset_arms_to_side(sim)
if args.animation:
viewer = trajoptpy.GetViewer(sim.env)
if os.path.isfile(args.window_prop_file) and os.path.isfile(
args.camera_matrix_file):
print "loading window and camera properties"
window_prop = np.loadtxt(args.window_prop_file)
camera_matrix = np.loadtxt(args.camera_matrix_file)
try:
viewer.SetWindowProp(*window_prop)
viewer.SetCameraManipulatorMatrix(camera_matrix)
except:
print "SetWindowProp and SetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."
else:
print "move viewer to viewpoint that isn't stupid"
print "then hit 'p' to continue"
viewer.Idle()
print "saving window and camera properties"
try:
window_prop = viewer.GetWindowProp()
camera_matrix = viewer.GetCameraManipulatorMatrix()
np.savetxt(args.window_prop_file, window_prop, fmt='%d')
np.savetxt(args.camera_matrix_file, camera_matrix)
except:
print "GetWindowProp and GetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."
viewer.Step()
if args.eval.dof_limits_factor != 1.0:
assert 0 < args.eval.dof_limits_factor and args.eval.dof_limits_factor <= 1.0
active_dof_indices = sim.robot.GetActiveDOFIndices()
active_dof_limits = sim.robot.GetActiveDOFLimits()
for lr in 'lr':
manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
dof_inds = sim.robot.GetManipulator(manip_name).GetArmIndices()
limits = np.asarray(sim.robot.GetDOFLimits(dof_inds))
limits_mean = limits.mean(axis=0)
limits_width = np.diff(limits, axis=0)
new_limits = limits_mean + args.eval.dof_limits_factor * \
np.r_[-limits_width / 2.0, limits_width / 2.0]
for i, ind in enumerate(dof_inds):
active_dof_limits[0][active_dof_indices.tolist().index(
ind)] = new_limits[0, i]
active_dof_limits[1][active_dof_indices.tolist().index(
ind)] = new_limits[1, i]
sim.robot.SetDOFLimits(active_dof_limits[0], active_dof_limits[1])
return lfd_env, sim
def label_demos_parallel(args, transferer, lfd_env, sim):
outfile = h5py.File(args.eval.outfile, 'a')
rope_params = sim_util.RopeParams()
rope_params.radius = settings.ROPE_RADIUS_THICK
if args.eval.rope_param_radius is not None:
rope_params.radius = args.eval.rope_param_radius
if args.eval.rope_param_angStiffness is not None:
rope_params.angStiffness = args.eval.rope_param_angStiffness
# TODO pass in rope params to sample_rope_state
(init_rope_nodes, demo_id) = sample_rope_state(args.eval, sim,
args.animation)
resample = False
labeled_data = []
while True:
if args.animation:
sim.viewer.Step()
sim_state = sim.get_state()
sim.set_state(sim_state)
scene_state = lfd_env.observe_scene()
# plot cloud of the test scene
handles = []
if args.plotting:
handles.append(
sim.env.plot3(
scene_state.cloud[:, :3], 2,
scene_state.color if scene_state.color is not None else
(0, 0, 1)))
sim.viewer.Step()
costs = transferer.registration_factory.batch_cost(scene_state)
best_keys = sorted(costs, key=costs.get)
for seg_name in best_keys:
traj = transferer.transfer(GlobalVars.demos[seg_name],
scene_state,
plotting=args.plotting)
feasible, misgrasp = lfd_env.execute_augmented_trajectory(
traj, step_viewer=args.animation, interactive=args.interactive)
sim_util.reset_arms_to_side(sim)
sim.settle(step_viewer=args.animation)
if not feasible or misgrasp:
print 'Feasible: ', feasible
print 'Misgrasp: ', misgrasp
if misgrasp:
sim.set_state(sim_state)
continue
print "y accepts this action"
print "n rejects this action"
print "r resamples rope state"
print "f to save this as a failure"
print "C-c safely quits"
user_input = lower(raw_input("What to do?"))
success = False
if user_input == 'y':
success = True
labeled_data.append((scene_state, seg_name))
if isFig8Knot(get_rope_nodes(sim)):
save_success(outfile, labeled_data)
labeled_data = []
sim.set_state(sample_rope_state(args.eval, sim))
break
elif user_input == 'n':
sim.set_state(sim_state)
continue
elif user_input == 'r':
break
elif user_input == 'f':
sim.set_state(sim_state)
save_failure(outfile, lfd_env.observe_scene())
continue
if not success:
labeled_data = []
sim.set_state(sample_rope_state(args.eval, sim))
cyl_radius,
cyl_height,
dynamic=True))
sim_objs.append(
CylinderSimulationObject("cyl1",
cyl_pos1,
cyl_radius,
cyl_height,
dynamic=True))
sim = DynamicSimulation()
sim.add_objects(sim_objs)
sim.create_viewer()
sim.robot.SetDOFValues(
[0.25], [sim.robot.GetJoint('torso_lift_joint').GetJointIndex()])
sim_util.reset_arms_to_side(sim)
# rotate cylinders by 90 deg
for i in range(2):
bt_cyl = sim.bt_env.GetObjectByName('cyl%d' % i)
T = openravepy.matrixFromAxisAngle(np.array([np.pi / 2, 0, 0]))
T[:3, 3] = bt_cyl.GetTransform()[:3, 3]
bt_cyl.SetTransform(
T
) # SetTransform needs to be used in the Bullet object, not the openrave body
sim.update()
sim.settle(max_steps=1000)
sim.viewer.Idle()
def eval_on_holdout(args, action_selection, reg_and_traj_transferer, lfd_env, sim):
"""TODO
Args:
action_selection: ActionSelection
reg_and_traj_transferer: RegistrationAndTrajectoryTransferer
lfd_env: LfdEnvironment
sim: DynamicSimulation
"""
holdoutfile = h5py.File(args.eval.holdoutfile, 'r')
holdout_items = eval_util.get_indexed_items(holdoutfile, task_list=args.tasks, task_file=args.taskfile, i_start=args.i_start, i_end=args.i_end)
rope_params = sim_util.RopeParams()
if args.eval.rope_param_radius is not None:
rope_params.radius = args.eval.rope_param_radius
if args.eval.rope_param_angStiffness is not None:
rope_params.angStiffness = args.eval.rope_param_angStiffness
num_successes = 0
num_total = 0
for i_task, demo_id_rope_nodes in holdout_items:
redprint("task %s" % i_task)
init_rope_nodes = demo_id_rope_nodes["rope_nodes"][:]
rope = RopeSimulationObject("rope", init_rope_nodes, rope_params)
sim.add_objects([rope])
sim.settle(step_viewer=args.animation)
for i_step in range(args.eval.num_steps):
redprint("task %s step %i" % (i_task, i_step))
sim_util.reset_arms_to_side(sim)
if args.animation:
sim.viewer.Step()
sim_state = sim.get_state()
sim.set_state(sim_state)
scene_state = lfd_env.observe_scene()
# plot cloud of the test scene
handles = []
if args.plotting:
handles.append(sim.env.plot3(scene_state.cloud[:,:3], 2, scene_state.color if scene_state.color is not None else (0,0,1)))
sim.viewer.Step()
eval_stats = eval_util.EvalStats()
start_time = time.time()
if len(scene_state.cloud) == 0:
redprint("Detected 0 points in scene")
break
try:
(agenda, q_values_root), goal_found = action_selection.plan_agenda(scene_state, i_step)
except ValueError: #e.g. if cloud is empty - any action is hopeless
redprint("**Raised Value Error during action selection")
break
eval_stats.action_elapsed_time += time.time() - start_time
eval_stats.generalized = True
num_actions_to_try = MAX_ACTIONS_TO_TRY if args.eval.search_until_feasible else 1
for i_choice in range(num_actions_to_try):
if q_values_root[i_choice] == -np.inf: # none of the demonstrations generalize
eval_stats.generalized = False
break
redprint("TRYING %s"%agenda[i_choice])
best_root_action = str(agenda[i_choice])
start_time = time.time()
try:
test_aug_traj = reg_and_traj_transferer.transfer(GlobalVars.demos[best_root_action], scene_state, plotting=args.plotting)
except ValueError: # If something is cloud/traj is empty or something
redprint("**Raised value error during traj transfer")
break
eval_stats.feasible, eval_stats.misgrasp = lfd_env.execute_augmented_trajectory(test_aug_traj, step_viewer=args.animation, interactive=args.interactive, check_feasible=args.eval.check_feasible)
eval_stats.exec_elapsed_time += time.time() - start_time
if not args.eval.check_feasible or eval_stats.feasible: # try next action if TrajOpt cannot find feasible action and we care about feasibility
break
else:
sim.set_state(sim_state)
knot = is_knot(rope.rope.GetControlPoints())
results = {'scene_state':scene_state, 'best_action':best_root_action, 'values':q_values_root, 'aug_traj':test_aug_traj, 'eval_stats':eval_stats, 'sim_state':sim_state, 'knot':knot, 'goal_found': goal_found}
eval_util.save_task_results_step(args.resultfile, i_task, i_step, results)
if not eval_stats.generalized:
assert not knot
break
if args.eval.check_feasible and not eval_stats.feasible:
# Skip to next knot tie if the action is infeasible -- since
# that means all future steps (up to 5) will have infeasible trajectories
assert not knot
break
if knot:
num_successes += 1
break;
sim.remove_objects([rope])
num_total += 1
redprint('Eval Successes / Total: ' + str(num_successes) + '/' + str(num_total))
redprint('Success Rate: ' + str(float(num_successes)/num_total))
def setUp(self):
table_height = 0.77
helix_ang0 = 0
helix_ang1 = 4 * np.pi
helix_radius = .2
helix_center = np.r_[.6, 0]
helix_height0 = table_height + .15
helix_height1 = table_height + .15 + .3
helix_length = np.linalg.norm(
np.r_[(helix_ang1 - helix_ang0) * helix_radius,
helix_height1 - helix_height0])
num = np.round(helix_length / .02)
helix_angs = np.linspace(helix_ang0, helix_ang1, num)
helix_heights = np.linspace(helix_height0, helix_height1, num)
init_rope_nodes = np.c_[helix_center +
helix_radius * np.c_[np.cos(helix_angs),
np.sin(helix_angs)],
helix_heights]
rope_params = sim_util.RopeParams()
cyl_radius = 0.025
cyl_height = 0.3
cyl_pos0 = np.r_[.6, helix_radius, table_height + .25]
cyl_pos1 = np.r_[.6, -helix_radius, table_height + .35]
sim_objs = []
sim_objs.append(
XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False))
sim_objs.append(
BoxSimulationObject("table", [1, 0, table_height - .1],
[.85, .85, .1],
dynamic=False))
sim_objs.append(
RopeSimulationObject("rope", init_rope_nodes, rope_params))
sim_objs.append(
CylinderSimulationObject("cyl0",
cyl_pos0,
cyl_radius,
cyl_height,
dynamic=True))
sim_objs.append(
CylinderSimulationObject("cyl1",
cyl_pos1,
cyl_radius,
cyl_height,
dynamic=True))
self.sim = DynamicSimulation()
self.sim.add_objects(sim_objs)
self.sim.robot.SetDOFValues(
[0.25],
[self.sim.robot.GetJoint('torso_lift_joint').GetJointIndex()])
sim_util.reset_arms_to_side(self.sim)
# rotate cylinders by 90 deg
for i in range(2):
bt_cyl = self.sim.bt_env.GetObjectByName('cyl%d' % i)
T = openravepy.matrixFromAxisAngle(np.array([np.pi / 2, 0, 0]))
T[:3, 3] = bt_cyl.GetTransform()[:3, 3]
bt_cyl.SetTransform(
T
) # SetTransform needs to be used in the Bullet object, not the openrave body
self.sim.update()
