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
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))
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
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()
