def initialize(load_gazebo=True):
openrave_sim = DynamicSimulationRobotWorld()
robot_xml = XmlSimulationObject("robots/pr2-beta-static.zae",
dynamic=False)
openrave_sim.add_objects([robot_xml], consider_finger_collisions=True)
table = BoxSimulationObject('table', \
[0.66, 0, (TABLE_HEIGHT + 0.03) / 2.0], \
[0.4, 0.75, (TABLE_HEIGHT + 0.03) / 2.0], \
dynamic=False)
openrave_sim.add_objects([table], consider_finger_collisions=True)
#table_xml = TableSimulationObject("table", \
# [0.66, 0, (TABLE_HEIGHT + 0.03) / 2.0],
# [0.4, 0.75, (TABLE_HEIGHT + 0.03) / 2.0],
# dynamic=False)
#openrave_sim.add_objects([table_xml], consider_finger_collisions=True)
cup = XmlSimulationObject(os.path.join(MODELS_DIR, CUP_MESH),
dynamic=False)
openrave_sim.add_objects([cup], consider_finger_collisions=True)
v = trajoptpy.GetViewer(
openrave_sim.env) # TODO: not sure if this is necessary
# Initialize interface for passing controls to Gazebo. Make sure to pass
# in the OpenRave environment we just created, so PR2 isn't referring (and
# updating) a separate OpenRave environment.
if load_gazebo:
pr2 = PR2.PR2(env=openrave_sim.env)
time.sleep(0.2)
else:
pr2 = None
return openrave_sim, pr2, v, cup
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 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 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 test_remove_sim_obj(self):
sim_state0 = self.sim.get_state()
self.sim.set_state(sim_state0)
self.sim.settle(max_steps=1000)
sim_state1 = self.sim.get_state()
rope = [
sim_obj for sim_obj in self.sim.sim_objs
if isinstance(sim_obj, RopeSimulationObject)
][0]
box = BoxSimulationObject("box", [0] * 3, [.1] * 3, dynamic=False)
self.sim.remove_objects([rope])
self.sim.set_state(sim_state0)
self.sim.settle(max_steps=1000)
sim_state2 = self.sim.get_state(
) # this adds another rope that has the same properties as rope
self.sim.add_objects([box])
self.sim.set_state(sim_state0)
self.sim.settle(max_steps=1000)
sim_state3 = self.sim.get_state(
) # this removes the recently added box
rope = [
sim_obj for sim_obj in self.sim.sim_objs
if isinstance(sim_obj, RopeSimulationObject)
][0]
self.sim.remove_objects([rope])
self.sim.add_objects([box])
self.sim.set_state(sim_state0)
self.sim.settle(max_steps=1000)
sim_state4 = self.sim.get_state()
self.assertArrayDictEqual(sim_state1[1], sim_state2[1])
self.assertArrayDictEqual(sim_state1[1], sim_state3[1])
self.assertArrayDictEqual(sim_state1[1], sim_state4[1])
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
#!/usr/bin/env python
from __future__ import division
import numpy as np
from lfd.environment.simulation import StaticSimulation
from lfd.environment.simulation_object import BoxSimulationObject
from lfd.registration.registration import TpsRpmRegistrationFactory
from lfd.registration.plotting_openrave import registration_plot_cb
from lfd.demonstration.demonstration import Demonstration, SceneState
np.random.seed(0)
table_height = 0.77
table = BoxSimulationObject("table", [1, 0, table_height - .1], [.85, .85, .1],
dynamic=False)
sim = StaticSimulation()
sim.add_objects([table])
sim.create_viewer()
def generate_cloud(x_center_pert=0, max_noise=0.02):
# generates 40 cm by 60 cm cloud with optional pertubation along the x-axis
grid = np.array(
np.meshgrid(np.linspace(-.2, .2, 21),
np.linspace(-.3, .3, 31))).T.reshape((-1, 2))
grid = np.c_[grid, np.zeros(len(grid))] + np.array(
[.5, 0, table_height + max_noise])
cloud = grid + x_center_pert * np.c_[
(0.3 - np.abs(grid[:, 1] - 0)) / 0.3,
from lfd.rapprentice import planning, resampling
from lfd.environment.simulation import DynamicSimulationRobotWorld
from lfd.environment.simulation_object import XmlSimulationObject, BoxSimulationObject
from lfd.environment import sim_util
table_height = 0.77
box_length = 0.04
box_depth = 0.12
box0_pos = np.r_[.6, -.3, table_height + box_depth / 2]
box1_pos = np.r_[.6, 0, table_height + box_depth / 2]
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(
BoxSimulationObject("box0",
box0_pos,
[box_length / 2, box_length / 2, box_depth / 2],
dynamic=True))
sim_objs.append(
BoxSimulationObject("box1",
box1_pos,
[box_length / 2, box_length / 2, box_depth / 2],
dynamic=True))
sim = DynamicSimulationRobotWorld()
sim.add_objects(sim_objs)
sim.create_viewer()
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()