def load_simulation(args, sim_env):
sim_env.env = openravepy.Environment()
sim_env.env.StopSimulation()
sim_env.env.Load("robots/pr2-beta-static.zae")
sim_env.robot = sim_env.env.GetRobots()[0]
actions = h5py.File(args.actionfile, 'r')
fakedatafile = h5py.File(args.fakedatafile, 'r')
GlobalVars.init_tfm = fakedatafile['init_tfm'][()]
init_rope_xyz, _ = sim_util.load_fake_data_segment(sim_env, fakedatafile, args.fake_data_segment, args.fake_data_transform) # this also sets the torso (torso_lift_joint) to the height in the data
# Set table height to correct height of first rope in holdout set
holdoutfile = h5py.File(args.holdoutfile, 'r')
first_holdout = holdoutfile[holdoutfile.keys()[0]]
init_rope_xyz = first_holdout['rope_nodes'][:]
if 'frame' not in first_holdout or first_holdout['frame'][()] != 'r':
init_rope_xyz = init_rope_xyz.dot(GlobalVars.init_tfm[:3,:3].T) + GlobalVars.init_tfm[:3,3][None,:]
table_height = init_rope_xyz[:,2].mean() - .02 # Before: .02
table_xml = sim_util.make_table_xml(translation=[1, 0, table_height], extents=[.85, .55, .01])
sim_env.env.LoadData(table_xml)
if 'bookshelve' in args.obstacles:
sim_env.env.Load("data/bookshelves.env.xml")
if 'boxes' in args.obstacles:
sim_env.env.LoadData(sim_util.make_box_xml("box0", [.7,.43,table_height+(.01+.12)], [.12,.12,.12]))
sim_env.env.LoadData(sim_util.make_box_xml("box1", [.74,.47,table_height+(.01+.12*2+.08)], [.08,.08,.08]))
cc = trajoptpy.GetCollisionChecker(sim_env.env)
for gripper_link in [link for link in sim_env.robot.GetLinks() if 'gripper' in link.GetName()]:
cc.ExcludeCollisionPair(gripper_link, sim_env.env.GetKinBody('table').GetLinks()[0])
sim_util.reset_arms_to_side(sim_env)
if args.animation:
sim_env.viewer = trajoptpy.GetViewer(sim_env.env)
if args.animation > 1 and 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:
sim_env.viewer.SetWindowProp(*window_prop)
sim_env.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"
sim_env.viewer.Idle()
print "saving window and camera properties"
try:
window_prop = sim_env.viewer.GetWindowProp()
camera_matrix = sim_env.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."
def load_simulation(args, sim_env):
sim_env.env = openravepy.Environment()
sim_env.env.StopSimulation()
actions = h5py.File(args.actionfile, 'r')
fakedatafile = h5py.File(args.fakedatafile, 'r')
GlobalVars.init_tfm = fakedatafile['init_tfm'][()]
init_rope_xyz, _ = sim_util.load_fake_data_segment(sim_env, fakedatafile, args.fake_data_segment, args.fake_data_transform, floating=True) # this also sets the torso (torso_lift_joint) to the height in the data
GlobalVars.init_tfm = GlobalVars.init_tfm
# Set table height to correct height of first rope in holdout set
holdoutfile = h5py.File(args.holdoutfile, 'r')
first_holdout = holdoutfile[holdoutfile.keys()[0]]
init_rope_xyz = first_holdout['rope_nodes'][:]
if 'frame' not in first_holdout or first_holdout['frame'][()] != 'r':
init_rope_xyz = init_rope_xyz.dot(GlobalVars.init_tfm[:3,:3].T) + GlobalVars.init_tfm[:3,3][None,:]
table_height = init_rope_xyz[:,2].mean() - .02 # Before: .02
GlobalVars.table_height = table_height
table_xml = sim_util.make_table_xml(translation=[1, 0, table_height], extents=[.85, .55, .01])
sim_env.env.LoadData(table_xml)
sim_util.reset_arms_to_side(sim_env, floating=True)
if args.animation:
sim_env.viewer = trajoptpy.GetViewer(sim_env.env)
table = sim_env.env.GetKinBody('table')
sim_env.viewer.SetTransparency(table, 0.4)
if args.animation > 1 and 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:
sim_env.viewer.SetWindowProp(*window_prop)
sim_env.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"
sim_env.viewer.Idle()
print "saving window and camera properties"
try:
window_prop = sim_env.viewer.GetWindowProp()
camera_matrix = sim_env.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."
def load_simulation(args, sim_env):
sim_env.env = openravepy.Environment()
sim_env.env.StopSimulation()
sim_env.env.Load("robots/pr2-beta-static.zae")
sim_env.robot = sim_env.env.GetRobots()[0]
actions = h5py.File(args.actionfile, 'r')
fakedatafile = h5py.File(args.fakedatafile, 'r')
GlobalVars.init_tfm = fakedatafile['init_tfm'][()]
init_rope_xyz, _ = sim_util.load_fake_data_segment(
sim_env, fakedatafile, args.fake_data_segment, args.fake_data_transform
) # this also sets the torso (torso_lift_joint) to the height in the data
# Set table height to correct height of first rope in holdout set
holdoutfile = h5py.File(args.holdoutfile, 'r')
first_holdout = holdoutfile[holdoutfile.keys()[0]]
init_rope_xyz = first_holdout['rope_nodes'][:]
if 'frame' not in first_holdout or first_holdout['frame'][()] != 'r':
init_rope_xyz = init_rope_xyz.dot(
GlobalVars.init_tfm[:3, :3].T) + GlobalVars.init_tfm[:3,
3][None, :]
table_height = init_rope_xyz[:, 2].mean() - .02 # Before: .02
table_xml = sim_util.make_table_xml(translation=[1, 0, table_height],
extents=[.85, .55, .01])
sim_env.env.LoadData(table_xml)
if 'bookshelve' in args.obstacles:
sim_env.env.Load("data/bookshelves.env.xml")
if 'boxes' in args.obstacles:
sim_env.env.LoadData(
sim_util.make_box_xml("box0",
[.7, .43, table_height + (.01 + .12)],
[.12, .12, .12]))
sim_env.env.LoadData(
sim_util.make_box_xml(
"box1", [.74, .47, table_height + (.01 + .12 * 2 + .08)],
[.08, .08, .08]))
cc = trajoptpy.GetCollisionChecker(sim_env.env)
for gripper_link in [
link for link in sim_env.robot.GetLinks()
if 'gripper' in link.GetName()
]:
cc.ExcludeCollisionPair(gripper_link,
sim_env.env.GetKinBody('table').GetLinks()[0])
sim_util.reset_arms_to_side(sim_env)
if args.animation:
sim_env.viewer = trajoptpy.GetViewer(sim_env.env)
if args.animation > 1 and 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:
sim_env.viewer.SetWindowProp(*window_prop)
sim_env.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"
sim_env.viewer.Idle()
print "saving window and camera properties"
try:
window_prop = sim_env.viewer.GetWindowProp()
camera_matrix = sim_env.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."
def __init__(self, name, translation, extents, dynamic=True):
xml = sim_util.make_table_xml(translation, extents)
super(TableSimulationObject, self).__init__(xml, dynamic=dynamic)
self.name = name
self.translation = translation
self.extents = extents