def main(): # The URDF loader seems robust to package:// and slightly wrong relative paths? connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") #with HideOutput(): with LockRenderer(): robot = load_model(MOVO_URDF, fixed_base=True) dump_body(robot) print('Start?') wait_for_user() #joint_names = HEAD_JOINTS #joints = joints_from_names(robot, joint_names) joints = get_movable_joints(robot) print('Joints', [get_joint_name(robot, joint) for joint in joints]) sample_fn = get_sample_fn(robot, joints) for i in range(10): print('Iteration:', i) conf = sample_fn() set_joint_positions(robot, joints, conf) wait_for_user() disconnect()
def clone_test(problem_fn): sim_world = connect(use_gui=True) add_data_path() problem = problem_fn() real_world = connect(use_gui=False) clone_world(real_world) set_client(real_world)
def main(): connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") with HideOutput(): with LockRenderer(): robot = load_model(FRANKA_URDF, fixed_base=True) dump_body(robot) print('Start?') wait_for_user() tool_link = link_from_name(robot, 'panda_hand') joints = get_movable_joints(robot) print('Joints', [get_joint_name(robot, joint) for joint in joints]) sample_fn = get_sample_fn(robot, joints) for i in range(10): print('Iteration:', i) conf = sample_fn() set_joint_positions(robot, joints, conf) wait_for_user() test_retraction(robot, PANDA_INFO, tool_link, max_distance=0.01, max_time=0.05) disconnect()
def main(): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-arm', required=True) parser.add_argument('-grasp', required=True) parser.add_argument('-viewer', action='store_true', help='enable viewer.') args = parser.parse_args() arm = args.arm other_arm = get_other_arm(arm) grasp_type = args.grasp connect(use_gui=args.viewer) add_data_path() with HideOutput(): robot = load_pybullet(DRAKE_PR2_URDF) set_group_conf(robot, 'torso', [0.2]) set_arm_conf(robot, arm, get_carry_conf(arm, grasp_type)) set_arm_conf(robot, other_arm, arm_conf(other_arm, REST_LEFT_ARM)) #plane = p.loadURDF("plane.urdf") #table = p.loadURDF("table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107) table = create_table() box = create_box(.07, .07, .14) #create_inverse_reachability(robot, box, table, arm=arm, grasp_type=grasp_type) create_inverse_reachability2(robot, box, table, arm=arm, grasp_type=grasp_type) disconnect()
def load_plane(z=-1e-3): add_data_path() plane = load_pybullet('plane.urdf', fixed_base=True) #plane = load_model('plane.urdf') if z is not None: set_point(plane, Point(z=z)) return plane
def main(): # The URDF loader seems robust to package:// and slightly wrong relative paths? connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") with LockRenderer(): with HideOutput(): robot = load_model(MOVO_URDF, fixed_base=True) for link in get_links(robot): set_color(robot, color=apply_alpha(0.25 * np.ones(3), 1), link=link) base_joints = joints_from_names(robot, BASE_JOINTS) draw_base_limits((get_min_limits( robot, base_joints), get_max_limits(robot, base_joints)), z=1e-2) dump_body(robot) wait_for_user('Start?') #for arm in ARMS: # test_close_gripper(robot, arm) arm = 'right' tool_link = link_from_name(robot, TOOL_LINK.format(arm)) #joint_names = HEAD_JOINTS #joints = joints_from_names(robot, joint_names) joints = base_joints + get_arm_joints(robot, arm) #joints = get_movable_joints(robot) print('Joints:', get_joint_names(robot, joints)) ik_joints = get_ik_joints(robot, MOVO_INFOS[arm], tool_link) #fixed_joints = [] fixed_joints = ik_joints[:1] #fixed_joints = ik_joints sample_fn = get_sample_fn(robot, joints) handles = [] for i in range(10): print('Iteration:', i) conf = sample_fn() print(conf) set_joint_positions(robot, joints, conf) tool_pose = get_link_pose(robot, tool_link) remove_handles(handles) handles = draw_pose(tool_pose) wait_for_user() #conf = next(ikfast_inverse_kinematics(robot, MOVO_INFOS[arm], tool_link, tool_pose, # fixed_joints=fixed_joints, max_time=0.1), None) #if conf is not None: # set_joint_positions(robot, ik_joints, conf) #wait_for_user() test_retraction(robot, MOVO_INFOS[arm], tool_link, fixed_joints=fixed_joints, max_time=0.1) disconnect()
def main(execute='apply'): #parser = argparse.ArgumentParser() # Automatically includes help #parser.add_argument('-display', action='store_true', help='enable viewer.') #args = parser.parse_args() #display = args.display display = True #display = False #filename = 'transporter.json' # simple | simple2 | cook | invert | kitchen | nonmonotonic_4_1 #problem_fn = lambda: load_json_problem(filename) problem_fn = cooking_problem # holding_problem | stacking_problem | cleaning_problem | cooking_problem # cleaning_button_problem | cooking_button_problem with HideOutput(): sim_world = connect(use_gui=False) set_client(sim_world) add_data_path() problem = problem_fn() print(problem) #state_id = save_state() if display: with HideOutput(): real_world = connect(use_gui=True) add_data_path() with ClientSaver(real_world): problem_fn() # TODO: way of doing this without reloading? update_state() wait_for_duration(0.1) #wait_for_interrupt() commands = plan_commands(problem) if (commands is None) or not display: with HideOutput(): disconnect() return time_step = 0.01 set_client(real_world) if execute == 'control': enable_gravity() control_commands(commands) elif execute == 'execute': step_commands(commands, time_step=time_step) elif execute == 'step': step_commands(commands) elif execute == 'apply': state = State() apply_commands(state, commands, time_step=time_step) else: raise ValueError(execute) wait_for_interrupt() with HideOutput(): disconnect()
def main(use_pr2_drake=True): connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") table_path = "models/table_collision/table.urdf" table = load_pybullet(table_path, fixed_base=True) set_quat(table, quat_from_euler(Euler(yaw=PI / 2))) # table/table.urdf, table_square/table_square.urdf, cube.urdf, block.urdf, door.urdf obstacles = [plane, table] pr2_urdf = DRAKE_PR2_URDF if use_pr2_drake else PR2_URDF with HideOutput(): pr2 = load_model(pr2_urdf, fixed_base=True) # TODO: suppress warnings? dump_body(pr2) z = base_aligned_z(pr2) print(z) #z = stable_z_on_aabb(pr2, AABB(np.zeros(3), np.zeros(3))) print(z) set_point(pr2, Point(z=z)) print(get_aabb(pr2)) wait_if_gui() base_start = (-2, -2, 0) base_goal = (2, 2, 0) arm_start = SIDE_HOLDING_LEFT_ARM #arm_start = TOP_HOLDING_LEFT_ARM #arm_start = REST_LEFT_ARM arm_goal = TOP_HOLDING_LEFT_ARM #arm_goal = SIDE_HOLDING_LEFT_ARM left_joints = joints_from_names(pr2, PR2_GROUPS['left_arm']) right_joints = joints_from_names(pr2, PR2_GROUPS['right_arm']) torso_joints = joints_from_names(pr2, PR2_GROUPS['torso']) set_joint_positions(pr2, left_joints, arm_start) set_joint_positions(pr2, right_joints, rightarm_from_leftarm(REST_LEFT_ARM)) set_joint_positions(pr2, torso_joints, [0.2]) open_arm(pr2, 'left') # test_ikfast(pr2) add_line(base_start, base_goal, color=RED) print(base_start, base_goal) if use_pr2_drake: test_drake_base_motion(pr2, base_start, base_goal, obstacles=obstacles) else: test_base_motion(pr2, base_start, base_goal, obstacles=obstacles) test_arm_motion(pr2, left_joints, arm_goal) # test_arm_control(pr2, left_joints, arm_start) wait_if_gui('Finish?') disconnect()
def create_world(): world = PlanningWorld(task=None, use_robot=False, visualize=True, color=BACKGROUND_COLOR, shadows=False) add_data_path() environment = [ #load_pybullet('models/short_floor.urdf'), #load_pybullet('plane.urdf', fixed_base=True), create_plane(color=GROUND_COLOR), ] set_camera_pose(camera_point=0.4*np.array([0, -1.0, 0.5])) #draw_pose(unit_pose(), length=1) return world
def clone_real_world_test(problem_fn): real_world = connect(use_gui=True) add_data_path() # world_file = 'test_world.py' # p.saveWorld(world_file) # Saves a Python file to be executed # state_id = p.saveState() # test_bullet = 'test_world.bullet' # save_bullet(test_bullet) # clone_world(real_world) with ClientSaver(real_world): # pass # restore_bullet(test_bullet) problem_fn() # TODO: way of doing this without reloading? update_state()
def list_pybullet_robots(): data_path = add_data_path() robot_path = os.path.abspath( os.path.join(data_path, os.pardir, 'pybullet_robots')) directories = sorted(name for name in os.listdir(robot_path) if os.path.isdir(os.path.join(robot_path, name))) return directories
def main(): # TODO: move to pybullet-planning for now parser = argparse.ArgumentParser() parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') args = parser.parse_args() print(args) connect(use_gui=True) with LockRenderer(): draw_pose(unit_pose(), length=1) floor = create_floor() with HideOutput(): robot = load_pybullet(get_model_path(ROOMBA_URDF), fixed_base=True, scale=2.0) for link in get_all_links(robot): set_color(robot, link=link, color=ORANGE) robot_z = stable_z(robot, floor) set_point(robot, Point(z=robot_z)) #set_base_conf(robot, rover_confs[i]) data_path = add_data_path() shelf, = load_model(os.path.join(data_path, KIVA_SHELF_SDF), fixed_base=True) # TODO: returns a tuple dump_body(shelf) #draw_aabb(get_aabb(shelf)) wait_for_user() disconnect()
def list_pybullet_data(): # TODO: recursively search data_path = add_data_path() #directories = sorted(name for name in os.listdir(data_path) if os.path.isdir(os.path.join(data_path, name))) files = sorted(name for name in os.listdir(data_path) if not os.path.isdir(os.path.join(data_path, name)) and ( os.path.splitext(name)[1] not in IGNORE_EXT)) return files
def main(use_pr2_drake=False): connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") #table_path = "table/table.urdf" # table_path = "models/table_collision/table.urdf" # table = p.loadURDF(table_path, 0, 0, 0, 0, 0, 0.707107, 0.707107) # table_square/table_square.urdf, cube.urdf, block.urdf, door.urdf pr2_urdf = DRAKE_PR2_URDF if use_pr2_drake else PR2_URDF with HideOutput(): pr2 = load_model(pr2_urdf, fixed_base=True) # TODO: suppress warnings? dump_body(pr2) base_start = (-2, -2, 0) base_goal = (2, 2, 0) arm_start = SIDE_HOLDING_LEFT_ARM #arm_start = TOP_HOLDING_LEFT_ARM #arm_start = REST_LEFT_ARM arm_goal = TOP_HOLDING_LEFT_ARM #arm_goal = SIDE_HOLDING_LEFT_ARM left_joints = joints_from_names(pr2, PR2_GROUPS['left_arm']) right_joints = joints_from_names(pr2, PR2_GROUPS['right_arm']) torso_joints = joints_from_names(pr2, PR2_GROUPS['torso']) set_joint_positions(pr2, left_joints, arm_start) set_joint_positions(pr2, right_joints, rightarm_from_leftarm(REST_LEFT_ARM)) set_joint_positions(pr2, torso_joints, [0.2]) open_arm(pr2, 'left') # test_ikfast(pr2) p.addUserDebugLine(base_start, base_goal, lineColorRGB=(1, 0, 0)) # addUserDebugText print(base_start, base_goal) if use_pr2_drake: test_drake_base_motion(pr2, base_start, base_goal) else: test_base_motion(pr2, base_start, base_goal) test_arm_motion(pr2, left_joints, arm_goal) # test_arm_control(pr2, left_joints, arm_start) user_input('Finish?') disconnect()
def main(): parser = argparse.ArgumentParser() parser.add_argument('-shape', default='box', choices=['box', 'sphere', 'cylinder', 'capsule']) parser.add_argument('-video', action='store_true') args = parser.parse_args() video = 'video.mp4' if args.video else None connect(use_gui=True, mp4=video) if video is not None: set_renderer(enable=False) draw_global_system() set_camera_pose(camera_point=Point(+1.5, -1.5, +1.5), target_point=Point(-1.5, +1.5, 0)) add_data_path() plane = load_pybullet('plane.urdf', fixed_base=True) #plane = load_model('plane.urdf') set_point(plane, Point(z=-1e-3)) ramp = create_ramp() dump_body(ramp) obj = create_object(args.shape) set_euler(obj, np.random.uniform(-np.math.radians(1), np.math.radians(1), 3)) set_point(obj, np.random.uniform(-1e-3, +1e-3, 3)) #set_velocity(obj, linear=Point(x=-1)) set_position(obj, z=2.) #set_position(obj, z=base_aligned_z(obj)) dump_body(obj) #add_pose_constraint(obj, pose=(target_point, target_quat), max_force=200) set_renderer(enable=True) if video is None: wait_if_gui('Begin?') simulate(controller=condition_controller(lambda step: (step != 0) and at_rest(obj))) if video is None: wait_if_gui('Finish?') disconnect()
def main(): connect(use_gui=True) add_data_path() draw_pose(Pose(), length=1.) set_camera_pose(camera_point=[1, -1, 1]) plane = p.loadURDF("plane.urdf") with LockRenderer(): with HideOutput(True): robot = load_pybullet(FRANKA_URDF, fixed_base=True) assign_link_colors(robot, max_colors=3, s=0.5, v=1.) #set_all_color(robot, GREEN) obstacles = [plane] # TODO: collisions with the ground dump_body(robot) print('Start?') wait_for_user() info = PANDA_INFO tool_link = link_from_name(robot, 'panda_hand') draw_pose(Pose(), parent=robot, parent_link=tool_link) joints = get_movable_joints(robot) print('Joints', [get_joint_name(robot, joint) for joint in joints]) check_ik_solver(info) sample_fn = get_sample_fn(robot, joints) for i in range(10): print('Iteration:', i) conf = sample_fn() set_joint_positions(robot, joints, conf) wait_for_user() #test_ik(robot, info, tool_link, get_link_pose(robot, tool_link)) test_retraction(robot, info, tool_link, use_pybullet=False, max_distance=0.1, max_time=0.05, max_candidates=100) disconnect()
def main(): # https://github.com/ros-teleop/teleop_twist_keyboard # http://openrave.org/docs/latest_stable/_modules/openravepy/misc/#SetViewerUserThread connect(use_gui=True) add_data_path() load_pybullet("plane.urdf") #load_pybullet("models/table_collision/table.urdf") with HideOutput(): pr2 = load_model(DRAKE_PR2_URDF, fixed_base=True) enable_gravity() enable_real_time( ) # TODO: won't work as well on OS X due to simulation thread #run_simulate(pr2) run_thread(pr2) # TODO: keep working on this #userthread = threading.Thread(target=run_thread, args=[pr2]) #userthread.start() #userthread.join() disconnect()
def create_table_bodies(world, item_ranges, randomize=True): perception = world.perception with HideOutput(): add_data_path() floor_body = load_pybullet("plane.urdf") set_pose(floor_body, get_pose(world.robot)) add_table(world) for name, limits in sorted(item_ranges.items()): perception.sim_items[name] = None if randomize: body = randomize_body(name, width_range=limits.width_range, height_range=limits.height_range, mass_range=limits.mass_range) else: body = load_body(name) perception.sim_bodies[name] = body # perception.add_item(name, unit_pose()) x, y, yaw = np.random.uniform(*limits.pose2d_range) surface_body = perception.get_body(limits.surface) z = stable_z(body, surface_body) + Z_EPSILON pose = Pose((x, y, z), Euler(yaw=yaw)) perception.set_pose(name, *pose)
def main(): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-arm', required=True) parser.add_argument('-grasp', required=True) parser.add_argument('-viewer', action='store_true', help='enable viewer.') args = parser.parse_args() arm = args.arm other_arm = get_other_arm(arm) grasp_type = args.grasp connect(use_gui=args.viewer) add_data_path() robot = p.loadURDF("models/pr2_description/pr2_fixed_torso.urdf") set_arm_conf(robot, arm, get_carry_conf(arm, grasp_type)) set_arm_conf(robot, other_arm, arm_conf(other_arm, REST_LEFT_ARM)) #plane = p.loadURDF("plane.urdf") table = p.loadURDF("table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107) box = create_box(.07, .05, .15) create_inverse_reachability(robot, box, table, arm=arm, grasp_type=grasp_type) disconnect()
def __init__(self, robot_name=FRANKA_CARTER, use_gui=True, full_kitchen=False): self.task = None self.interface = None self.client = connect(use_gui=use_gui) set_real_time(False) #set_caching(False) # Seems to make things worse disable_gravity() add_data_path() set_camera_pose(camera_point=[2, -1.5, 1]) if DEBUG: draw_pose(Pose(), length=3) #self.kitchen_yaml = load_yaml(KITCHEN_YAML) with HideOutput(enable=True): self.kitchen = load_pybullet(KITCHEN_PATH, fixed_base=True, cylinder=True) self.floor = load_pybullet('plane.urdf', fixed_base=True) z = stable_z(self.kitchen, self.floor) - get_point(self.floor)[2] point = np.array(get_point(self.kitchen)) - np.array([0, 0, z]) set_point(self.floor, point) self.robot_name = robot_name if self.robot_name == FRANKA_CARTER: urdf_path, yaml_path = FRANKA_CARTER_PATH, None #urdf_path, yaml_path = FRANKA_CARTER_PATH, FRANKA_YAML #elif self.robot_name == EVE: # urdf_path, yaml_path = EVE_PATH, None else: raise ValueError(self.robot_name) self.robot_yaml = yaml_path if yaml_path is None else load_yaml( yaml_path) with HideOutput(enable=True): self.robot = load_pybullet(urdf_path) #dump_body(self.robot) #chassis_pose = get_link_pose(self.robot, link_from_name(self.robot, 'chassis_link')) #wheel_pose = get_link_pose(self.robot, link_from_name(self.robot, 'left_wheel_link')) #wait_for_user() set_point(self.robot, Point(z=stable_z(self.robot, self.floor))) self.set_initial_conf() self.gripper = create_gripper(self.robot) self.environment_bodies = {} if full_kitchen: self._initialize_environment() self._initialize_ik(urdf_path) self.initial_saver = WorldSaver() self.body_from_name = {} # self.path_from_name = {} self.names_from_type = {} self.custom_limits = {} self.base_limits_handles = [] self.cameras = {} self.disabled_collisions = set() if self.robot_name == FRANKA_CARTER: self.disabled_collisions.update( tuple(link_from_name(self.robot, link) for link in pair) for pair in DISABLED_FRANKA_COLLISIONS) self.carry_conf = FConf(self.robot, self.arm_joints, self.default_conf) #self.calibrate_conf = Conf(self.robot, self.arm_joints, load_calibrate_conf(side='left')) self.calibrate_conf = FConf( self.robot, self.arm_joints, self.default_conf) # Must differ from carry_conf self.special_confs = [self.carry_conf] #, self.calibrate_conf] self.open_gq = FConf(self.robot, self.gripper_joints, get_max_limits(self.robot, self.gripper_joints)) self.closed_gq = FConf(self.robot, self.gripper_joints, get_min_limits(self.robot, self.gripper_joints)) self.gripper_confs = [self.open_gq, self.closed_gq] self.open_kitchen_confs = { joint: FConf(self.kitchen, [joint], [self.open_conf(joint)]) for joint in self.kitchen_joints } self.closed_kitchen_confs = { joint: FConf(self.kitchen, [joint], [self.closed_conf(joint)]) for joint in self.kitchen_joints } self._update_custom_limits() self._update_initial()
def main(): connect(use_gui=True) add_data_path() set_camera(0, -30, 1) plane = load_pybullet('plane.urdf', fixed_base=True) #plane = load_model('plane.urdf') cup = load_model('models/cup.urdf', fixed_base=True) #set_point(cup, Point(z=stable_z(cup, plane))) set_point(cup, Point(z=.2)) set_color(cup, (1, 0, 0, .4)) num_droplets = 100 #radius = 0.025 #radius = 0.005 radius = 0.0025 # TODO: more efficient ways to make all of these droplets = [create_sphere(radius, mass=0.01) for _ in range(num_droplets)] # kg cup_thickness = 0.001 lower, upper = get_lower_upper(cup) print(lower, upper) buffer = cup_thickness + radius lower = np.array(lower) + buffer * np.ones(len(lower)) upper = np.array(upper) - buffer * np.ones(len(upper)) limits = zip(lower, upper) x_range, y_range = limits[:2] z = upper[2] + 0.1 #x_range = [-1, 1] #y_range = [-1, 1] #z = 1 for droplet in droplets: x = np.random.uniform(*x_range) y = np.random.uniform(*y_range) set_point(droplet, Point(x, y, z)) for i, droplet in enumerate(droplets): x, y = np.random.normal(0, 1e-3, 2) set_point(droplet, Point(x, y, z + i * (2 * radius + 1e-3))) #dump_world() wait_for_user() #user_input('Start?') enable_gravity() simulate_for_duration(5.0) # enable_real_time() # try: # while True: # enable_gravity() # enable_real_time requires a command # #time.sleep(dt) # except KeyboardInterrupt: # pass # print() #time.sleep(1.0) wait_for_user('Finish?') disconnect()
def main(num_iterations=10): # The URDF loader seems robust to package:// and slightly wrong relative paths? connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") side = 0.05 block = create_box(w=side, l=side, h=side, color=RED) start_time = time.time() with LockRenderer(): with HideOutput(): # TODO: MOVO must be loaded last robot = load_model(MOVO_URDF, fixed_base=True) #set_all_color(robot, color=MOVO_COLOR) assign_link_colors(robot) base_joints = joints_from_names(robot, BASE_JOINTS) draw_base_limits((get_min_limits( robot, base_joints), get_max_limits(robot, base_joints)), z=1e-2) print('Load time: {:.3f}'.format(elapsed_time(start_time))) dump_body(robot) #print(get_colliding(robot)) #for arm in ARMS: # test_close_gripper(robot, arm) #test_grasps(robot, block) arm = RIGHT tool_link = link_from_name(robot, TOOL_LINK.format(arm)) #joint_names = HEAD_JOINTS #joints = joints_from_names(robot, joint_names) joints = base_joints + get_arm_joints(robot, arm) #joints = get_movable_joints(robot) print('Joints:', get_joint_names(robot, joints)) ik_info = MOVO_INFOS[arm] print_ik_warning(ik_info) ik_joints = get_ik_joints(robot, ik_info, tool_link) #fixed_joints = [] fixed_joints = ik_joints[:1] #fixed_joints = ik_joints wait_if_gui('Start?') sample_fn = get_sample_fn(robot, joints) handles = [] for i in range(num_iterations): conf = sample_fn() print('Iteration: {}/{} | Conf: {}'.format(i + 1, num_iterations, np.array(conf))) set_joint_positions(robot, joints, conf) tool_pose = get_link_pose(robot, tool_link) remove_handles(handles) handles = draw_pose(tool_pose) wait_if_gui() #conf = next(ikfast_inverse_kinematics(robot, MOVO_INFOS[arm], tool_link, tool_pose, # fixed_joints=fixed_joints, max_time=0.1), None) #if conf is not None: # set_joint_positions(robot, ik_joints, conf) #wait_if_gui() test_retraction(robot, ik_info, tool_link, fixed_joints=fixed_joints, max_time=0.05, max_candidates=100) disconnect()
def main(): # TODO: teleporting kuka arm parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-viewer', action='store_true', help='enable viewer.') args = parser.parse_args() client = connect(use_gui=args.viewer) add_data_path() #planeId = p.loadURDF("plane.urdf") table = p.loadURDF("table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107) box = create_box(.07, .05, .15) # boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation) #pr2 = p.loadURDF("pr2_description/pr2.urdf") pr2 = p.loadURDF("pr2_description/pr2_fixed_torso.urdf") #pr2 = p.loadURDF("/Users/caelan/Programs/Installation/pr2_drake/pr2_local2.urdf",) #useFixedBase=0,) #flags=p.URDF_USE_SELF_COLLISION) #flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT) #flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) #pr2 = p.loadURDF("pr2_drake/urdf/pr2_simplified.urdf", useFixedBase=False) initially_colliding = get_colliding_links(pr2) print len(initially_colliding) origin = (0, 0, 0) print p.getNumConstraints() # TODO: no way of controlling the base position by itself # TODO: PR2 seems to collide with itself frequently # real_time = False # p.setRealTimeSimulation(real_time) # left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] # control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM) # while True: # control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM) # if not real_time: # p.stepSimulation() # A CollisionMap robot allows the user to specify self-collision regions indexed by the values of two joints. # GetRigidlyAttachedLinks print pr2 # for i in range (10000): # p.stepSimulation() # time.sleep(1./240.) #print get_joint_names(pr2) print[get_joint_name(pr2, joint) for joint in get_movable_joints(pr2)] print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME)) #open_gripper(pr2, joint_from_name(pr2, LEFT_GRIPPER)) #print get_joint_limits(pr2, joint_from_name(pr2, LEFT_GRIPPER)) #print get_joint_position(pr2, joint_from_name(pr2, LEFT_GRIPPER)) print self_collision(pr2) """ print p.getNumConstraints() constraint = fixed_constraint(pr2, -1, box, -1) # table p.changeConstraint(constraint) print p.getNumConstraints() print p.getConstraintInfo(constraint) print p.getConstraintState(constraint) p.stepSimulation() raw_input('Continue?') set_point(pr2, (-2, 0, 0)) p.stepSimulation() p.changeConstraint(constraint) print p.getConstraintInfo(constraint) print p.getConstraintState(constraint) raw_input('Continue?') print get_point(pr2) raw_input('Continue?') """ # TODO: would be good if we could set the joint directly print set_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME), 0.2) # Updates automatically print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME)) #return left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] right_joints = [joint_from_name(pr2, name) for name in RIGHT_JOINT_NAMES] print set_joint_positions( pr2, left_joints, TOP_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM print set_joint_positions( pr2, right_joints, REST_RIGHT_ARM) # TOP_HOLDING_RIGHT_ARM | REST_RIGHT_ARM print get_body_name(pr2) print get_body_names() # print p.getBodyUniqueId(pr2) print get_joint_names(pr2) #for joint, value in zip(LEFT_ARM_JOINTS, REST_LEFT_ARM): # set_joint_position(pr2, joint, value) # for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM): # joint = joint_from_name(pr2, name) # #print name, joint, get_joint_position(pr2, joint), value # print name, get_joint_limits(pr2, joint), get_joint_type(pr2, joint), get_link_name(pr2, joint) # set_joint_position(pr2, joint, value) # #print name, joint, get_joint_position(pr2, joint), value # for name, value in zip(RIGHT_JOINT_NAMES, REST_RIGHT_ARM): # set_joint_position(pr2, joint_from_name(pr2, name), value) print p.getNumJoints(pr2) jointId = 0 print p.getJointInfo(pr2, jointId) print p.getJointState(pr2, jointId) # for i in xrange(10): # #lower, upper = BASE_LIMITS # #q = np.random.rand(len(lower))*(np.array(upper) - np.array(lower)) + lower # q = np.random.uniform(*BASE_LIMITS) # theta = np.random.uniform(*REVOLUTE_LIMITS) # quat = z_rotation(theta) # print q, theta, quat, env_collision(pr2) # #set_point(pr2, q) # set_pose(pr2, q, quat) # #p.getMouseEvents() # #p.getKeyboardEvents() # raw_input('Continue?') # Stalls because waiting for input # # # TODO: self collisions # for i in xrange(10): # for name in LEFT_JOINT_NAMES: # joint = joint_from_name(pr2, name) # value = np.random.uniform(*get_joint_limits(pr2, joint)) # set_joint_position(pr2, joint, value) # raw_input('Continue?') #start = (-2, -2, 0) #set_base_values(pr2, start) # #start = get_base_values(pr2) # goal = (2, 2, 0) # p.addUserDebugLine(start, goal, lineColorRGB=(1, 1, 0)) # addUserDebugText # print start, goal # raw_input('Plan?') # path = plan_base_motion(pr2, goal) # print path # if path is None: # return # print len(path) # for bq in path: # set_base_values(pr2, bq) # raw_input('Continue?') # left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] # for joint in left_joints: # print joint, get_joint_name(pr2, joint), get_joint_limits(pr2, joint), \ # is_circular(pr2, joint), get_joint_position(pr2, joint) # # #goal = np.zeros(len(left_joints)) # goal = [] # for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM): # joint = joint_from_name(pr2, name) # goal.append(wrap_joint(pr2, joint, value)) # # path = plan_joint_motion(pr2, left_joints, goal) # print path # for q in path:s # set_joint_positions(pr2, left_joints, q) # raw_input('Continue?') print p.JOINT_REVOLUTE, p.JOINT_PRISMATIC, p.JOINT_FIXED, p.JOINT_POINT2POINT, p.JOINT_GEAR # 0 1 4 5 6 movable_joints = get_movable_joints(pr2) print len(movable_joints) for joint in xrange(get_num_joints(pr2)): if is_movable(pr2, joint): print joint, get_joint_name(pr2, joint), get_joint_type( pr2, joint), get_joint_limits(pr2, joint) #joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] #set_joint_positions(pr2, joints, sample_joints(pr2, joints)) #print get_joint_positions(pr2, joints) # Need to print before the display updates? # set_base_values(pr2, (1, -1, -np.pi/4)) # movable_joints = get_movable_joints(pr2) # gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK)) # print gripper_pose # print get_joint_positions(pr2, movable_joints) # p.addUserDebugLine(origin, gripper_pose[0], lineColorRGB=(1, 0, 0)) # p.stepSimulation() # raw_input('Pre2 IK') # set_joint_positions(pr2, left_joints, SIDE_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM # print get_joint_positions(pr2, movable_joints) # p.stepSimulation() # raw_input('Pre IK') # conf = inverse_kinematics(pr2, gripper_pose) # Doesn't automatically set configuraitons # print conf # print get_joint_positions(pr2, movable_joints) # set_joint_positions(pr2, movable_joints, conf) # print get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK)) # #print get_joint_positions(pr2, movable_joints) # p.stepSimulation() # raw_input('Post IK') # return # print pose_from_tform(TOOL_TFORM) # gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK)) # #gripper_pose = multiply(gripper_pose, TOOL_POSE) # #gripper_pose = get_gripper_pose(pr2) # for i, grasp_pose in enumerate(get_top_grasps(box)): # grasp_pose = multiply(TOOL_POSE, grasp_pose) # box_pose = multiply(gripper_pose, grasp_pose) # set_pose(box, *box_pose) # print get_pose(box) # raw_input('Grasp {}'.format(i)) # return torso = joint_from_name(pr2, TORSO_JOINT_NAME) torso_point, torso_quat = get_link_pose(pr2, torso) #torso_constraint = p.createConstraint(pr2, torso, -1, -1, # p.JOINT_FIXED, jointAxis=[0] * 3, # JOINT_FIXED # parentFramePosition=torso_point, # childFramePosition=torso_quat) create_inverse_reachability(pr2, box, table) ir_database = load_inverse_reachability() print len(ir_database) return link = link_from_name(pr2, LEFT_ARM_LINK) point, quat = get_link_pose(pr2, link) print point, quat p.addUserDebugLine(origin, point, lineColorRGB=(1, 1, 0)) # addUserDebugText raw_input('Continue?') current_conf = get_joint_positions(pr2, movable_joints) #ik_conf = p.calculateInverseKinematics(pr2, link, point) #ik_conf = p.calculateInverseKinematics(pr2, link, point, quat) min_limits = [get_joint_limits(pr2, joint)[0] for joint in movable_joints] max_limits = [get_joint_limits(pr2, joint)[1] for joint in movable_joints] max_velocities = [ get_max_velocity(pr2, joint) for joint in movable_joints ] # Range of Jacobian print min_limits print max_limits print max_velocities ik_conf = p.calculateInverseKinematics(pr2, link, point, quat, lowerLimits=min_limits, upperLimits=max_limits, jointRanges=max_velocities, restPoses=current_conf) value_from_joint = dict(zip(movable_joints, ik_conf)) print[value_from_joint[joint] for joint in joints] #print len(ik_conf), ik_conf set_joint_positions(pr2, movable_joints, ik_conf) #print len(movable_joints), get_joint_positions(pr2, movable_joints) print get_joint_positions(pr2, joints) raw_input('Finish?') p.disconnect()