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)
traj, _, _ = planning.plan_follow_traj(sim.robot, manip_name, ee_link, new_hmats, old_traj, start_fixed=start_fixed, beta_rot=10000.0)
return traj
pick_pos = rope_pos0 + .1 * (rope_pos1 - rope_pos0)
drop_pos = rope_pos3 + .1 * (rope_pos2 - rope_pos3) + 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
dof_inds = sim_util.dof_inds_from_name(sim.robot, manip_name)
traj = get_move_traj(pick_pos + np.r_[0,0,move_height], pick_pos, pick_R, pick_R, False)
sim.execute_trajectory((traj, dof_inds))
sim.close_gripper(lr)
traj = get_move_traj(pick_pos, pick_pos + np.r_[0,0,move_height], pick_R, pick_R, True)
sim.execute_trajectory((traj, dof_inds))
traj = get_move_traj(pick_pos + np.r_[0,0,move_height], drop_pos + np.r_[0,0,move_height], pick_R, drop_R, True)
sim.execute_trajectory((traj, dof_inds))
traj = get_move_traj(drop_pos + np.r_[0,0,move_height], drop_pos, drop_R, drop_R, True)
sim.execute_trajectory((traj, dof_inds))
sim.open_gripper(lr)
traj = get_move_traj(drop_pos, drop_pos + np.r_[0,0,move_height], drop_R, drop_R, True)
sim.execute_trajectory((traj, dof_inds))
sim.settle(max_steps=10000)
viewer.Idle()
hmat_end = np.r_[np.c_[R, t_end], np.c_[0,0,0,1]]
new_hmats = np.asarray(resampling.interp_hmats(np.arange(n_steps), np.r_[0, n_steps-1], [hmat_start, hmat_end]))
dof_vals = sim.robot.GetManipulator(manip_name).GetArmDOFValues()
old_traj = np.tile(dof_vals, (n_steps,1))
traj, _, _ = planning.plan_follow_traj(sim.robot, manip_name, ee_link, new_hmats, old_traj, start_fixed=start_fixed, beta_rot=10000.0)
return traj
move_height = .3
dof_inds = sim_util.dof_inds_from_name(sim.robot, manip_name)
traj = get_move_traj(box0_pos + np.r_[0,0,move_height], box0_pos + np.r_[0,0,box_depth/2-0.02], False)
sim.execute_trajectory((traj, dof_inds))
sim.close_gripper(lr)
traj = get_move_traj(box0_pos + np.r_[0,0,box_depth/2-0.02], box0_pos + np.r_[0,0,move_height], True)
sim.execute_trajectory((traj, dof_inds))
traj = get_move_traj(box0_pos + np.r_[0,0,move_height], box1_pos + np.r_[0,0,move_height], True)
sim.execute_trajectory((traj, dof_inds))
traj = get_move_traj(box1_pos + np.r_[0,0,move_height], box1_pos + np.r_[0,0,box_depth+box_depth/2-0.02+0.001], True)
sim.execute_trajectory((traj, dof_inds))
sim.open_gripper(lr)
traj = get_move_traj(box1_pos + np.r_[0,0,box_depth+box_depth/2-0.02+0.002], box1_pos + np.r_[0,0,move_height], True)
sim.execute_trajectory((traj, dof_inds))
sim.settle()
sim.viewer.Idle()
move_height = .3
dof_inds = sim_util.dof_inds_from_name(sim.robot, manip_name)
traj = get_move_traj(box0_pos + np.r_[0, 0, move_height],
box0_pos + np.r_[0, 0, box_depth / 2 - 0.02], False)
sim.execute_trajectory((traj, dof_inds))
sim.close_gripper(lr)
traj = get_move_traj(box0_pos + np.r_[0, 0, box_depth / 2 - 0.02],
box0_pos + np.r_[0, 0, move_height], True)
sim.execute_trajectory((traj, dof_inds))
traj = get_move_traj(box0_pos + np.r_[0, 0, move_height],
box1_pos + np.r_[0, 0, move_height], True)
sim.execute_trajectory((traj, dof_inds))
traj = get_move_traj(
box1_pos + np.r_[0, 0, move_height],
box1_pos + np.r_[0, 0, box_depth + box_depth / 2 - 0.02 + 0.001], True)
sim.execute_trajectory((traj, dof_inds))
sim.open_gripper(lr)
traj = get_move_traj(
box1_pos + np.r_[0, 0, box_depth + box_depth / 2 - 0.02 + 0.002],
box1_pos + np.r_[0, 0, move_height], True)
sim.execute_trajectory((traj, dof_inds))
sim.settle()
sim.viewer.Idle()