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