def main(display='control'): # control | execute | step connect(use_gui=True) disable_real_time() # KUKA_IIWA_URDF | DRAKE_IIWA_URDF robot = load_model(DRAKE_IIWA_URDF, fixed_base=True) # floor = load_model('models/short_floor.urdf') floor = p.loadURDF("plane.urdf") block = load_model( "models/drake/objects/block_for_pick_and_place_heavy.urdf", fixed_base=False) set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor)))) set_default_camera() dump_world() saved_world = WorldSaver() command = plan(robot, block, fixed=[floor], teleport=False) if (command is None) or (display is None): print('Unable to find a plan!') return saved_world.restore() update_state() user_input('{}?'.format(display)) if display == 'control': enable_gravity() command.control(real_time=False, dt=0) elif display == 'execute': command.refine(num_steps=10).execute(time_step=0.005) elif display == 'step': command.step() else: raise ValueError(display) print('Quit?') wait_for_user() disconnect()
def main(display='execute'): # control | execute | step connect(use_gui=True) disable_real_time() draw_global_system() with HideOutput(): robot = load_model(DRAKE_IIWA_URDF) # KUKA_IIWA_URDF | DRAKE_IIWA_URDF floor = load_model('models/short_floor.urdf') block = load_model(BLOCK_URDF, fixed_base=False) set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor)))) set_default_camera(distance=2) dump_world() saved_world = WorldSaver() command = plan(robot, block, fixed=[floor], teleport=False) if (command is None) or (display is None): print('Unable to find a plan!') return saved_world.restore() update_state() wait_if_gui('{}?'.format(display)) if display == 'control': enable_gravity() command.control(real_time=False, dt=0) elif display == 'execute': command.refine(num_steps=10).execute(time_step=0.005) elif display == 'step': command.step() else: raise ValueError(display) print('Quit?') wait_if_gui() disconnect()
def main(display='execute'): # control | execute | step # One of the arm's gantry carriage is fixed when the other is moving. connect(use_gui=True) set_camera(yaw=-90, pitch=-40, distance=10, target_position=(0, 7.5, 0)) draw_pose(unit_pose(), length=1.0) disable_real_time() with HideOutput(): root_directory = os.path.dirname(os.path.abspath(__file__)) robot = load_pybullet(os.path.join(root_directory, ETH_RFL_URDF), fixed_base=True) # floor = load_model('models/short_floor.urdf') block = load_model(BLOCK_URDF, fixed_base=False) #link = link_from_name(robot, 'gantry_base_link') #print(get_aabb(robot, link)) block_x = -0.2 #block_y = 1 if ARM == 'right' else 13.5 #block_x = 10 block_y = 5. # set_pose(floor, Pose(Point(x=floor_x, y=1, z=1.3))) # set_pose(block, Pose(Point(x=floor_x, y=0.6, z=stable_z(block, floor)))) set_pose(block, Pose(Point(x=block_x, y=block_y, z=3.5))) # set_default_camera() dump_world() #print(get_camera()) saved_world = WorldSaver() with LockRenderer(): command = plan(robot, block, fixed=[], teleport=False) # fixed=[floor], if (command is None) or (display is None): print('Unable to find a plan! Quit?') wait_for_interrupt() disconnect() return saved_world.restore() update_state() print('{}?'.format(display)) wait_for_interrupt() if display == 'control': enable_gravity() command.control(real_time=False, dt=0) elif display == 'execute': command.refine(num_steps=10).execute(time_step=0.002) elif display == 'step': command.step() else: raise ValueError(display) print('Quit?') wait_for_interrupt() disconnect()
def test_clone_arm(pr2): first_joint_name = PR2_GROUPS['left_arm'][0] first_joint = joint_from_name(pr2, first_joint_name) parent_joint = get_link_parent(pr2, first_joint) print(get_link_name(pr2, parent_joint), parent_joint, first_joint_name, first_joint) print(get_link_descendants(pr2, first_joint)) # TODO: least common ancestor links = [first_joint] + get_link_descendants(pr2, first_joint) new_arm = clone_body(pr2, links=links, collision=False) dump_world() set_base_values(pr2, (-2, 0, 0)) wait_for_interrupt()
def test_clone_robot(pr2): # TODO: j toggles frames, p prints timings, w is wire, a is boxes new_pr2 = clone_body(pr2, visual=True, collision=False) #new_pr2 = clone_body_editor(pr2, visual=True, collision=True) dump_world() #print(load_srdf_collisions()) #print(load_dae_collisions()) # TODO: some unimportant quats are off for both URDF and other # TODO: maybe all the frames are actually correct when I load things this way? # TODO: the visual geometries are off but not the collision frames? """ import numpy as np for link in get_links(pr2): if not get_visual_data(new_pr2, link): # pybullet.error: Error receiving visual shape info? continue #print(get_link_name(pr2, link)) data1 = VisualShapeData(*get_visual_data(pr2, link)[0]) data2 = VisualShapeData(*get_visual_data(new_pr2, link)[0]) pose1 = (data1.localVisualFrame_position, data1.localVisualFrame_orientation) pose2 = (data2.localVisualFrame_position, data2.localVisualFrame_orientation) #pose1 = get_link_pose(pr2, link) # Links are fine #pose2 = get_link_pose(new_pr2, link) #pose1 = get_joint_parent_frame(pr2, link) #pose2 = get_joint_parent_frame(new_pr2, link) #pose1 = get_joint_inertial_pose(pr2, link) # Inertia is fine #pose2 = get_joint_inertial_pose(new_pr2, link) if not np.allclose(pose1[0], pose2[0], rtol=0, atol=1e-3): print('Point', get_link_name(pr2, link), link, pose1[0], pose2[0]) #print(data1) #print(data2) #print(get_joint_parent_frame(pr2, link), get_joint_parent_frame(new_pr2, link)) print(get_joint_inertial_pose(pr2, link)) #, get_joint_inertial_pose(new_pr2, link)) print() if not np.allclose(euler_from_quat(pose1[1]), euler_from_quat(pose2[1]), rtol=0, atol=1e-3): print('Quat', get_link_name(pr2, link), link, euler_from_quat(pose1[1]), euler_from_quat(pose2[1])) joint_info1 = get_joint_info(pr2, link) joint_info2 = get_joint_info(new_pr2, link) # TODO: the axis is off for some of these if not np.allclose(joint_info1.jointAxis, joint_info2.jointAxis, rtol=0, atol=1e-3): print('Axis', get_link_name(pr2, link), link, joint_info1.jointAxis, joint_info2.jointAxis) """ set_base_values(new_pr2, (2, 0, 0)) wait_for_interrupt()
def main(display='execute'): # control | execute | step connect(use_gui=True) disable_real_time() with HideOutput(): root_directory = os.path.dirname(os.path.abspath(__file__)) robot = load_pybullet(os.path.join(root_directory, IRB6600_TRACK_URDF), fixed_base=True) floor = load_model('models/short_floor.urdf') block = load_model(BLOCK_URDF, fixed_base=False) floor_x = 2 set_pose(floor, Pose(Point(x=floor_x, z=0.5))) set_pose(block, Pose(Point(x=floor_x, y=0, z=stable_z(block, floor)))) # set_default_camera() dump_world() saved_world = WorldSaver() with LockRenderer(): command = plan(robot, block, fixed=[floor], teleport=False) if (command is None) or (display is None): print('Unable to find a plan!') print('Quit?') wait_for_interrupt() disconnect() return saved_world.restore() update_state() user_input('{}?'.format(display)) if display == 'control': enable_gravity() command.control(real_time=False, dt=0) elif display == 'execute': command.refine(num_steps=10).execute(time_step=0.002) elif display == 'step': command.step() else: raise ValueError(display) print('Quit?') wait_for_interrupt() disconnect()
def main(execute='execute'): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-viewer', action='store_true', help='enable viewer.') #parser.add_argument('-display', action='store_true', help='enable viewer.') args = parser.parse_args() #display = args.display display = True connect(use_gui=args.viewer) robot, block = load_world() #robot2 = clone_body(robot) #block2 = clone_body(block) #dump_world() saved_world = WorldSaver() dump_world() ss_problem = ss_from_problem(robot, movable=[block], teleport=False, movable_collisions=True) #ss_problem = ss_problem.debug_problem() #print(ss_problem) t0 = time.time() plan, evaluations = dual_focused(ss_problem, verbose=True) # plan, evaluations = incremental(ss_problem, verbose=True) print_plan(plan, evaluations) print(time.time() - t0) if (not display) or (plan is None): disconnect() return paths = [] for action, params in plan: if action.name == 'place': paths += params[-1].reverse().body_paths elif action.name in ['move_free', 'move_holding', 'pick']: paths += params[-1].body_paths print(paths) command = Command(paths) if not args.viewer: # TODO: how to reenable the viewer disconnect() connect(use_gui=True) load_world() saved_world.restore() user_input('Execute?') if execute == 'control': command.control() elif execute == 'execute': command.refine(num_steps=10).execute(time_step=0.001) elif execute == 'step': command.step() else: raise ValueError(execute) #dt = 1. / 240 # Bullet default #p.setTimeStep(dt) wait_for_interrupt() disconnect()