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)
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)
viewer.SetCameraManipulatorMatrix(camera_matrix)
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, np.zeros((len(grid),2))] + (np.random.random((len(grid), 3)) - 0.5) * 2 * max_noise
return cloud
demos = {}
for x_center_pert in np.arange(-0.1, 0.6, 0.1):
demo_name = "demo_{}".format(x_center_pert)
demo_cloud = generate_cloud(x_center_pert=x_center_pert)
demo_scene_state = SceneState(demo_cloud, downsample_size=0.025)
demo = Demonstration(demo_name, demo_scene_state, None)
demos[demo_name] = demo
test_cloud = generate_cloud(x_center_pert=0.2)
test_scene_state = SceneState(test_cloud, downsample_size=0.025)
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_factory = TpsRpmRegistrationFactory(demos)
regs = reg_factory.batch_register(test_scene_state, callback=plot_cb)
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,
np.zeros((len(grid), 2))] + (np.random.random(
(len(grid), 3)) - 0.5) * 2 * max_noise
return cloud
demos = {}
for x_center_pert in np.arange(-0.1, 0.6, 0.1):
demo_name = "demo_{}".format(x_center_pert)
demo_cloud = generate_cloud(x_center_pert=x_center_pert)
demo_scene_state = SceneState(demo_cloud, downsample_size=0.025)
demo = Demonstration(demo_name, demo_scene_state, None)
demos[demo_name] = demo
test_cloud = generate_cloud(x_center_pert=0.2)
test_scene_state = SceneState(test_cloud, downsample_size=0.025)
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_factory = TpsRpmRegistrationFactory(demos)
regs = reg_factory.batch_register(test_scene_state, callback=plot_cb)
