def main(): # define simulation objects table_height = 0.77 cyl_radius = 0.025 cyl_height = 0.3 cyl_pos0 = np.r_[0.7, -0.15, table_height + cyl_height / 2] cyl_pos1 = np.r_[0.7, 0.15, table_height + cyl_height / 2] cyl_pos2 = np.r_[0.4, -0.15, table_height + cyl_height / 2] rope_poss = np.array( [ [0.2, -0.2, table_height + 0.006], [0.8, -0.2, table_height + 0.006], [0.8, 0.2, table_height + 0.006], [0.2, 0.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 - 0.1], [0.85, 0.85, 0.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] + 0.1 * (rope_poss[1] - rope_poss[0]) drop_pos = rope_poss[3] + 0.1 * (rope_poss[2] - rope_poss[3]) + np.r_[0, 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 = 0.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 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 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)
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() sim.robot.SetDOFValues([0.25], [sim.robot.GetJoint('torso_lift_joint').GetJointIndex()]) sim_util.reset_arms_to_side(sim) # rotate box0 by 22.5 degrees bt_box0 = sim.bt_env.GetObjectByName('box0') T = openravepy.matrixFromAxisAngle(np.array([0,0,np.pi/8])) T[:3,3] = bt_box0.GetTransform()[:3,3] bt_box0.SetTransform(T) # SetTransform needs to be used in the Bullet object, not the openrave body sim.update() lr = 'r' manip_name = {"l":"leftarm", "r":"rightarm"}[lr] ee_link_name = "%s_gripper_tool_frame"%lr ee_link = sim.robot.GetLink(ee_link_name)
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() sim.robot.SetDOFValues( [0.25], [sim.robot.GetJoint('torso_lift_joint').GetJointIndex()]) sim_util.reset_arms_to_side(sim) # rotate box0 by 22.5 degrees bt_box0 = sim.bt_env.GetObjectByName('box0') T = openravepy.matrixFromAxisAngle(np.array([0, 0, np.pi / 8])) T[:3, 3] = bt_box0.GetTransform()[:3, 3] bt_box0.SetTransform( T ) # SetTransform needs to be used in the Bullet object, not the openrave body sim.update() lr = 'r'