def setup_lfd_environment_sim(args):
actions = h5py.File(args.gen_tasks.actionfile, 'r')
init_rope_xyz, init_joint_names, init_joint_values = sim_util.load_fake_data_segment(
actions, args.gen_tasks.fake_data_segment,
args.gen_tasks.fake_data_transform)
table_height = init_rope_xyz[:, 2].mean() - .02
sim_objs = []
sim_objs.append(
BoxSimulationObject("table", [1, 0, table_height + (-.1 + .01)],
[.85, .85, .1],
dynamic=False))
sim = DynamicSimulation()
world = sim
sim.add_objects(sim_objs)
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()
return sim
def setup_lfd_environment_sim(args):
actions = h5py.File(args.gen_tasks.actionfile, 'r')
init_rope_xyz, init_joint_names, init_joint_values = sim_util.load_fake_data_segment(actions, args.gen_tasks.fake_data_segment, args.gen_tasks.fake_data_transform)
table_height = init_rope_xyz[:,2].mean() - .02
sim_objs = []
sim_objs.append(BoxSimulationObject("table",
[1, 0, table_height + (-.1 + .01)],
[.85, .85, .1], dynamic=False))
sim = DynamicSimulation()
world = sim
sim.add_objects(sim_objs)
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()
return 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
