def main(): connect(use_gui=True) with HideOutput(): pr2 = load_model( "models/drake/pr2_description/urdf/pr2_simplified.urdf") set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['left_arm']), REST_LEFT_ARM) set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['right_arm']), rightarm_from_leftarm(REST_LEFT_ARM)) set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['torso']), [0.2]) dump_body(pr2) block = load_model(BLOCK_URDF, fixed_base=False) set_point(block, [2, 0.5, 1]) target_point = point_from_pose(get_pose(block)) # head_link = link_from_name(pr2, HEAD_LINK) head_joints = joints_from_names(pr2, PR2_GROUPS['head']) head_link = head_joints[-1] #max_detect_distance = 2.5 max_register_distance = 1.0 distance_range = (max_register_distance / 2, max_register_distance) base_generator = visible_base_generator(pr2, target_point, distance_range) base_joints = joints_from_names(pr2, PR2_GROUPS['base']) for i in range(5): base_conf = next(base_generator) set_joint_positions(pr2, base_joints, base_conf) p.addUserDebugLine(point_from_pose(get_link_pose(pr2, head_link)), target_point, lineColorRGB=(1, 0, 0)) # addUserDebugText p.addUserDebugLine(point_from_pose( get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))), target_point, lineColorRGB=(0, 0, 1)) # addUserDebugText # head_conf = sub_inverse_kinematics(pr2, head_joints[0], HEAD_LINK, ) head_conf = inverse_visibility(pr2, target_point) set_joint_positions(pr2, head_joints, head_conf) print(get_detections(pr2)) # TODO: does this detect the robot sometimes? detect_mesh, z = get_detection_cone(pr2, block) detect_cone = create_mesh(detect_mesh, color=(0, 1, 0, 0.5)) set_pose(detect_cone, get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))) view_cone = get_viewcone(depth=2.5, color=(1, 0, 0, 0.25)) set_pose(view_cone, get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))) wait_for_user() remove_body(detect_cone) remove_body(view_cone) disconnect()
def get_gripper_joints(robot, arm): assert arm in ARMS if has_kg3_gripper(robot, arm): return joints_from_names(robot, names_from_templates(KG3_GRIPPER_JOINTS, arm)) elif has_robotiq_gripper(robot, arm): return joints_from_names( robot, names_from_templates(ROBOTIQ_GRIPPER_JOINTS, arm)) raise ValueError(arm)
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 run_simulate(pr2): joints = joints_from_names(pr2, PR2_GROUPS['base']) dx = dy = dz = dth = 1 speed, turn = 0.5, 1.0 while True: velocities = [dx * speed, dy * speed, dth * turn] velocity_control_joints(pr2, joints, velocities)
def run_thread(pr2): joints = joints_from_names(pr2, PR2_GROUPS['base']) dx = dy = dz = dth = 0 speed, turn = 0.5, 1.0 settings = termios.tcgetattr(sys.stdin) try: print(HELP_MSG) print_velocities(speed, turn) while True: key = get_key(settings) # Waits until a key is read if key in MOVE_BINDINGS: dx, dy, dz, dth = MOVE_BINDINGS[key] elif key in SPEED_BINDINGS: mspeed, mturn = SPEED_BINDINGS[key] speed *= mspeed turn *= mturn print_velocities(speed, turn) else: # When it receives another key dx = dy = dz = dth = 0 if key == ESCAPE: break # twist.linear.dz = dz * speed velocities = [dx * speed, dy * speed, dth * turn] velocity_control_joints(pr2, joints, velocities) except Exception as e: print(e) finally: termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
def gripper_joints(self): #if self.robot_yaml is None: # return [] joint_names = ['panda_finger_joint{}'.format(1 + i) for i in range(2)] #joint_names = [joint_from_name(self.robot, rule['name']) # for rule in self.robot_yaml['cspace_to_urdf_rules']] return joints_from_names(self.robot, joint_names)
def kitchen_joints(self): joint_names = get_joint_names(self.kitchen, get_movable_joints(self.kitchen)) #joint_names = self.kitchen_yaml['cspace'] #return joints_from_names(self.kitchen, joint_names) return joints_from_names(self.kitchen, filter(ALL_JOINTS.__contains__, joint_names))
def execute(self, controller, joint_speed): if not self.path: return # if self.teleport: # for _ in self.iterate(controller.world, attachments={}): # pass arm_prefix = get_arm_prefix(self.arm) robot = controller.world.robot arm_joints = joints_from_names( robot, controller.get_arm_joint_names(arm_prefix)) current_positions = controller.get_arm_positions(arm_prefix) # For debugging #modification = [random.randint(-10, +10) * np.pi if is_circular(robot, joint) else 0 for joint in arm_joints] #current_positions = current_positions + np.array(modification) difference_fn = get_difference_fn(robot, arm_joints) differences = [ difference_fn(q2, q1) for q1, q2 in zip(self.path, self.path[1:]) ] adjusted_path = [np.array(current_positions)] for difference in differences: adjusted_path.append(adjusted_path[-1] + difference) #adjusted_path = adjust_trajectory(self.path, current_positions) #for i, (q1, q2) in enumerate(zip(self.path, trajectory)): # print(i, (np.array(q2) - np.array(q1)).round(3).tolist()) times_from_start = generate_times(adjusted_path, joint_speed / self.dialation) controller.command_arm_trajectory( arm_prefix, adjusted_path[1:], times_from_start=times_from_start[1:]) controller.command_arm(arm_prefix, adjusted_path[-1], timeout=1.0) return True
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(): # 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 test_ikfast(pr2): from pybullet_tools.ikfast.pr2.ik import get_tool_pose, get_ik_generator 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']) torso_left = torso_joints + left_joints print(get_link_pose(pr2, link_from_name(pr2, 'l_gripper_tool_frame'))) # print(forward_kinematics('left', get_joint_positions(pr2, torso_left))) print(get_tool_pose(pr2, 'left')) arm = 'left' pose = get_tool_pose(pr2, arm) generator = get_ik_generator(pr2, arm, pose, torso_limits=False) for i in range(100): solutions = next(generator) print(i, len(solutions)) for q in solutions: set_joint_positions(pr2, torso_left, q) wait_for_user()
def command_gripper(self, arm, position, max_effort=NULL_EFFORT, timeout=2.0, blocking=True): # position is the width of the gripper as in the physical distance between the two fingers with ClientSaver(self.client): gripper_joints = joints_from_names( self.robot, self.get_gripper_joint_names(arm)) positions = [position] * len(gripper_joints) self.follow_trajectory(gripper_joints, [positions], max_sim_duration=timeout)
def command_arm_trajectory(self, arm, path, times_from_start, blocking=True, **kwargs): # angles is a list of joint angles, times is a list of times from start # When calling joints on an arm, needs to be called with all the angles in the arm # times is not used because our pybullet's position controller doesn't take into account times assert len(path) == len(times_from_start) with ClientSaver(self.client): arm_joints = joints_from_names(self.robot, self.get_arm_joint_names(arm)) self.follow_trajectory(arm_joints, path, times_from_start, **kwargs)
def problem1(n_obstacles=10, wall_side=0.1, obst_width=0.25, obst_height=0.5): floor_extent = 5.0 base_limits = (-floor_extent/2.*np.ones(2), +floor_extent/2.*np.ones(2)) floor_height = 0.001 floor = create_box(floor_extent, floor_extent, floor_height, color=TAN) set_point(floor, Point(z=-floor_height/2.)) wall1 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY) set_point(wall1, Point(y=floor_extent/2., z=wall_side/2.)) wall2 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY) set_point(wall2, Point(y=-floor_extent/2., z=wall_side/2.)) wall3 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY) set_point(wall3, Point(x=floor_extent/2., z=wall_side/2.)) wall4 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY) set_point(wall4, Point(x=-floor_extent/2., z=wall_side/2.)) walls = [wall1, wall2, wall3, wall4] initial_surfaces = OrderedDict() for _ in range(n_obstacles): body = create_box(obst_width, obst_width, obst_height, color=GREY) initial_surfaces[body] = floor obstacles = walls + list(initial_surfaces) initial_conf = np.array([+floor_extent/3, -floor_extent/3, 3*PI/4]) goal_conf = -initial_conf with HideOutput(): robot = load_model(TURTLEBOT_URDF) base_joints = joints_from_names(robot, BASE_JOINTS) # base_link = child_link_from_joint(base_joints[-1]) base_link = link_from_name(robot, BASE_LINK_NAME) set_all_color(robot, BLUE) dump_body(robot) set_point(robot, Point(z=stable_z(robot, floor))) draw_pose(Pose(), parent=robot, parent_link=base_link, length=0.5) set_joint_positions(robot, base_joints, initial_conf) sample_placements(initial_surfaces, obstacles=[robot] + walls, savers=[BodySaver(robot, joints=base_joints, positions=goal_conf)], min_distances=10e-2) return robot, base_limits, goal_conf, obstacles
def solve_trac_ik(self, world_from_tool, nearby_tolerance=INF): assert self.ik_solver is not None init_lower, init_upper = self.ik_solver.get_joint_limits() base_link = link_from_name(self.robot, self.ik_solver.base_link) world_from_base = get_link_pose(self.robot, base_link) tip_link = link_from_name(self.robot, self.ik_solver.tip_link) tool_from_tip = multiply( invert(get_link_pose(self.robot, self.tool_link)), get_link_pose(self.robot, tip_link)) world_from_tip = multiply(world_from_tool, tool_from_tip) base_from_tip = multiply(invert(world_from_base), world_from_tip) joints = joints_from_names( self.robot, self.ik_solver.joint_names) # self.ik_solver.link_names seed_state = get_joint_positions(self.robot, joints) # seed_state = [0.0] * self.ik_solver.number_of_joints lower, upper = init_lower, init_upper if nearby_tolerance < INF: tolerance = nearby_tolerance * np.ones(len(joints)) lower = np.maximum(lower, seed_state - tolerance) upper = np.minimum(upper, seed_state + tolerance) self.ik_solver.set_joint_limits(lower, upper) (x, y, z), (rx, ry, rz, rw) = base_from_tip # TODO: can also adjust tolerances conf = self.ik_solver.get_ik(seed_state, x, y, z, rx, ry, rz, rw) self.ik_solver.set_joint_limits(init_lower, init_upper) if conf is None: return conf # if nearby_tolerance < INF: # print(lower.round(3)) # print(upper.round(3)) # print(conf) # print(get_difference(seed_state, conf).round(3)) set_joint_positions(self.robot, joints, conf) return get_configuration(self.robot)
def plan(robot, block, fixed, teleport): grasp_gen = get_grasp_gen(robot, 'bottom', get_tool_frame(ARM)) #'top' ik_fn = get_ik_fn(robot, fixed=fixed, teleport=teleport, self_collisions=ENABLE_SELF_COLLISION) free_motion_fn = get_free_motion_gen(robot, fixed=([block] + fixed), teleport=teleport, self_collisions=ENABLE_SELF_COLLISION) holding_motion_fn = get_holding_motion_gen(robot, fixed=fixed, teleport=teleport, self_collisions=ENABLE_SELF_COLLISION) #arm_joints = get_torso_arm_joints(robot, ARM) arm_joints = joints_from_names(robot, get_arm_joint_names(ARM)) pose0 = BodyPose(block) conf0 = BodyConf(robot, joints=arm_joints) saved_world = WorldSaver() for grasp, in grasp_gen(block): saved_world.restore() result1 = ik_fn(block, pose0, grasp) if result1 is None: print('ik fn fails!') continue conf1, path2 = result1 pose0.assign() result2 = free_motion_fn(conf0, conf1) if result2 is None: print("free motion plan fails!") continue path1, = result2 result3 = holding_motion_fn(conf1, conf0, block, grasp) if result3 is None: print('holding motion fails!') continue path3, = result3 return Command(path1.body_paths + path2.body_paths + path3.body_paths) return None
def main(): # TODO: update this example connect(use_gui=True) with HideOutput(): pr2 = load_model(DRAKE_PR2_URDF) set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['left_arm']), REST_LEFT_ARM) set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['right_arm']), rightarm_from_leftarm(REST_LEFT_ARM)) set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['torso']), [0.2]) dump_body(pr2) block = load_model(BLOCK_URDF, fixed_base=False) set_point(block, [2, 0.5, 1]) target_point = point_from_pose(get_pose(block)) draw_point(target_point) head_joints = joints_from_names(pr2, PR2_GROUPS['head']) #head_link = child_link_from_joint(head_joints[-1]) #head_name = get_link_name(pr2, head_link) head_name = 'high_def_optical_frame' # HEAD_LINK_NAME | high_def_optical_frame | high_def_frame head_link = link_from_name(pr2, head_name) #max_detect_distance = 2.5 max_register_distance = 1.0 distance_range = (max_register_distance / 2, max_register_distance) base_generator = visible_base_generator(pr2, target_point, distance_range) base_joints = joints_from_names(pr2, PR2_GROUPS['base']) for i in range(5): base_conf = next(base_generator) set_joint_positions(pr2, base_joints, base_conf) handles = [ add_line(point_from_pose(get_link_pose(pr2, head_link)), target_point, color=RED), add_line(point_from_pose( get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))), target_point, color=BLUE), ] # head_conf = sub_inverse_kinematics(pr2, head_joints[0], HEAD_LINK, ) head_conf = inverse_visibility(pr2, target_point, head_name=head_name, head_joints=head_joints) assert head_conf is not None set_joint_positions(pr2, head_joints, head_conf) print(get_detections(pr2)) # TODO: does this detect the robot sometimes? detect_mesh, z = get_detection_cone(pr2, block) detect_cone = create_mesh(detect_mesh, color=(0, 1, 0, 0.5)) set_pose(detect_cone, get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))) view_cone = get_viewcone(depth=2.5, color=(1, 0, 0, 0.25)) set_pose(view_cone, get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))) wait_if_gui() remove_body(detect_cone) remove_body(view_cone) disconnect()
def get_track_joint(robot): return joints_from_names(robot, TRACK_JOINT)
def get_track_arm_joints(robot): return joints_from_names(robot, TRACK_ARM_JOINT)
def base_joints(self): return joints_from_names(self.robot, BASE_JOINTS)
def arm_joints(self): #if self.robot_name == EVE: # return get_eve_arm_joints(self.robot, arm=DEFAULT_ARM) joint_names = ['panda_joint{}'.format(1 + i) for i in range(7)] #joint_names = self.robot_yaml['cspace'] return joints_from_names(self.robot, joint_names)
def get_joint_positions(self, joint_names): # Returns the configuration of the specified joints with ClientSaver(self.client): joints = joints_from_names(self.robot, joint_names) return get_joint_positions(self.robot, joints)
def set_joint_positions(self, joint_names, positions): with ClientSaver(self.client): joints = joints_from_names(self.robot, joint_names) set_joint_positions(self.robot, joints, positions)
def problem1(n_obstacles=10, wall_side=0.1, obst_width=0.25, obst_height=0.5): floor_extent = 5.0 base_limits = (-floor_extent / 2. * np.ones(2), +floor_extent / 2. * np.ones(2)) floor_height = 0.001 floor = create_box(floor_extent, floor_extent, floor_height, color=TAN) set_point(floor, Point(z=-floor_height / 2.)) wall1 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY) set_point(wall1, Point(y=floor_extent / 2., z=wall_side / 2.)) wall2 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY) set_point(wall2, Point(y=-floor_extent / 2., z=wall_side / 2.)) wall3 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY) set_point(wall3, Point(x=floor_extent / 2., z=wall_side / 2.)) wall4 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY) set_point(wall4, Point(x=-floor_extent / 2., z=wall_side / 2.)) wall5 = create_box(obst_width, obst_width, obst_height, color=GREY) set_point(wall5, Point(z=obst_height / 2.)) walls = [wall1, wall2, wall3, wall4, wall5] initial_surfaces = OrderedDict() for _ in range(n_obstacles - 1): body = create_box(obst_width, obst_width, obst_height, color=GREY) initial_surfaces[body] = floor pillars = list(initial_surfaces) obstacles = walls + pillars initial_conf = np.array([+floor_extent / 3, -floor_extent / 3, 3 * PI / 4]) goal_conf = -initial_conf robot = load_pybullet(TURTLEBOT_URDF, fixed_base=True, merge=True, sat=False) base_joints = joints_from_names(robot, BASE_JOINTS) # base_link = child_link_from_joint(base_joints[-1]) base_link = link_from_name(robot, BASE_LINK_NAME) set_all_color(robot, BLUE) dump_body(robot) set_point(robot, Point(z=stable_z(robot, floor))) #set_point(robot, Point(z=base_aligned_z(robot))) draw_pose(Pose(), parent=robot, parent_link=base_link, length=0.5) set_joint_positions(robot, base_joints, initial_conf) sample_placements( initial_surfaces, obstacles=[robot] + walls, savers=[BodySaver(robot, joints=base_joints, positions=goal_conf)], min_distances=10e-2) # The first calls appear to be the slowest # times = [] # for body1, body2 in combinations(pillars, r=2): # start_time = time.time() # colliding = pairwise_collision(body1, body2) # runtime = elapsed_time(start_time) # print(colliding, runtime) # times.append(runtime) # print(times) return robot, base_limits, goal_conf, obstacles
def get_arm_joints(robot, arm): assert arm in ARMS return joints_from_names(robot, names_from_templates(ARM_JOINTS, arm))
def main(): np.set_printoptions(precision=N_DIGITS, suppress=True, threshold=3) # , edgeitems=1) #, linewidth=1000) parser = argparse.ArgumentParser() parser.add_argument( '-a', '--algorithm', default='rrt_connect', # choices=[], help='The motion planning algorithm to use.') parser.add_argument('-c', '--cfree', action='store_true', help='When enabled, disables collision checking.') # parser.add_argument('-p', '--problem', default='test_pour', choices=sorted(problem_fn_from_name), # help='The name of the problem to solve.') parser.add_argument( '--holonomic', action='store_true', # '-h', help='') parser.add_argument('-l', '--lock', action='store_false', help='') parser.add_argument('-r', '--reversible', action='store_true', help='') parser.add_argument( '-s', '--seed', default=None, type=int, # None | 1 help='The random seed to use.') parser.add_argument('-n', '--num', default=10, type=int, help='The number of obstacles.') parser.add_argument('-o', '--orientation', action='store_true', help='') parser.add_argument('-v', '--viewer', action='store_false', help='') args = parser.parse_args() connect(use_gui=args.viewer) #set_aabb_buffer(buffer=1e-3) #set_separating_axis_collisions() #seed = 0 #seed = time.time() seed = args.seed if seed is None: seed = random.randint(0, 10**3 - 1) print('Seed:', seed) set_random_seed(seed=seed) # None: 2147483648 = 2**31 set_numpy_seed(seed=seed) #print('Random seed:', get_random_seed(), random.random()) #print('Numpy seed:', get_numpy_seed(), np.random.random()) ######################### robot, base_limits, goal_conf, obstacles = problem1(n_obstacles=args.num) custom_limits = create_custom_base_limits(robot, base_limits) base_joints = joints_from_names(robot, BASE_JOINTS) draw_base_limits(base_limits) # draw_pose(get_link_pose(robot, base_link), length=0.5) start_conf = get_joint_positions(robot, base_joints) for conf in [start_conf, goal_conf]: draw_waypoint(conf) #resolutions = None #resolutions = np.array([0.05, 0.05, math.radians(10)]) plan_joints = base_joints[:2] if not args.orientation else base_joints d = len(plan_joints) holonomic = args.holonomic or (d != 3) resolutions = 1. * DEFAULT_RESOLUTION * np.ones( d) # TODO: default resolutions, velocities, accelerations fns #weights = np.reciprocal(resolutions) weights = np.array([1, 1, 1e-3])[:d] cost_fn = get_acceleration_fn(robot, plan_joints, max_velocities=MAX_VELOCITIES[:d], max_accelerations=MAX_ACCELERATIONS[:d]) # TODO: check that taking shortest turning direction (reversible affecting?) if args.cfree: obstacles = [] # for obstacle in obstacles: # draw_aabb(get_aabb(obstacle)) # Updates automatically #set_all_static() # Doesn't seem to affect #test_aabb(robot) #test_caching(robot, obstacles) #return ######################### # TODO: filter if straight-line path is feasible saver = WorldSaver() wait_for_duration(duration=1e-2) start_time = time.time() with LockRenderer(lock=args.lock): with Profiler(field='cumtime', num=25): # tottime | cumtime | None # TODO: draw the search tree path = plan_base_joint_motion( robot, plan_joints, goal_conf[:d], holonomic=holonomic, reversible=args.reversible, obstacles=obstacles, self_collisions=False, custom_limits=custom_limits, use_aabb=True, cache=True, max_distance=MIN_PROXIMITY, resolutions=resolutions, weights=weights, # TODO: use KDTrees circular={ 2: UNBOUNDED_LIMITS if holonomic else CIRCULAR_LIMITS }, cost_fn=cost_fn, algorithm=args.algorithm, verbose=True, restarts=5, max_iterations=50, smooth=None if holonomic else 100, smooth_time=1, # None | 100 | INF ) saver.restore() # TODO: draw ROADMAPS #wait_for_duration(duration=1e-3) ######################### solved = path is not None length = INF if path is None else len(path) cost = compute_cost(robot, base_joints, path, resolutions=resolutions[:len(plan_joints)]) print( 'Solved: {} | Length: {} | Cost: {:.3f} | Runtime: {:.3f} sec'.format( solved, length, cost, elapsed_time(start_time))) if path is None: wait_if_gui() disconnect() return # for i, conf in enumerate(path): # set_joint_positions(robot, plan_joints, conf) # wait_if_gui('{}/{}) Continue?'.format(i + 1, len(path))) path = extract_full_path(robot, plan_joints, path, base_joints) with LockRenderer(): draw_last_roadmap(robot, base_joints) # for i, conf in enumerate(path): # set_joint_positions(robot, base_joints, conf) # wait_if_gui('{}/{}) Continue?'.format(i+1, len(path))) iterate_path( robot, base_joints, path, step_size=2e-2, smooth=holonomic, custom_limits=custom_limits, resolutions=DEFAULT_RESOLUTION * np.ones(3), # resolutions obstacles=obstacles, self_collisions=False, max_distance=MIN_PROXIMITY) disconnect()
def main(): parser = argparse.ArgumentParser() parser.add_argument('-c', '--cfree', action='store_true', help='When enabled, disables collision checking.') # parser.add_argument('-p', '--problem', default='test_pour', choices=sorted(problem_fn_from_name), # help='The name of the problem to solve.') parser.add_argument('--holonomic', action='store_true', # '-h', help='') parser.add_argument('-l', '--lock', action='store_false', help='') parser.add_argument('-s', '--seed', default=None, type=int, help='The random seed to use.') parser.add_argument('-v', '--viewer', action='store_false', help='') args = parser.parse_args() connect(use_gui=args.viewer) seed = args.seed #seed = 0 #seed = time.time() set_random_seed(seed=seed) # None: 2147483648 = 2**31 set_numpy_seed(seed=seed) print('Random seed:', get_random_seed(), random.random()) print('Numpy seed:', get_numpy_seed(), np.random.random()) ######################### robot, base_limits, goal_conf, obstacles = problem1() draw_base_limits(base_limits) custom_limits = create_custom_base_limits(robot, base_limits) base_joints = joints_from_names(robot, BASE_JOINTS) base_link = link_from_name(robot, BASE_LINK_NAME) if args.cfree: obstacles = [] # for obstacle in obstacles: # draw_aabb(get_aabb(obstacle)) # Updates automatically resolutions = None #resolutions = np.array([0.05, 0.05, math.radians(10)]) set_all_static() # Doesn't seem to affect region_aabb = AABB(lower=-np.ones(3), upper=+np.ones(3)) draw_aabb(region_aabb) step_simulation() # Need to call before get_bodies_in_region #update_scene() # TODO: https://github.com/bulletphysics/bullet3/pull/3331 bodies = get_bodies_in_region(region_aabb) print(len(bodies), bodies) # https://github.com/bulletphysics/bullet3/search?q=broadphase # https://github.com/bulletphysics/bullet3/search?p=1&q=getCachedOverlappingObjects&type=&utf8=%E2%9C%93 # https://andysomogyi.github.io/mechanica/bullet.html # http://www.cs.kent.edu/~ruttan/GameEngines/lectures/Bullet_User_Manual #draw_pose(get_link_pose(robot, base_link), length=0.5) for conf in [get_joint_positions(robot, base_joints), goal_conf]: draw_pose(pose_from_pose2d(conf, z=DRAW_Z), length=DRAW_LENGTH) aabb = get_aabb(robot) #aabb = get_subtree_aabb(robot, base_link) draw_aabb(aabb) for link in [BASE_LINK, base_link]: print(link, get_collision_data(robot, link), pairwise_link_collision(robot, link, robot, link)) ######################### saver = WorldSaver() start_time = time.time() profiler = Profiler(field='tottime', num=50) # tottime | cumtime | None profiler.save() with LockRenderer(lock=args.lock): path = plan_motion(robot, base_joints, goal_conf, holonomic=args.holonomic, obstacles=obstacles, custom_limits=custom_limits, resolutions=resolutions, use_aabb=True, cache=True, max_distance=0., restarts=2, iterations=20, smooth=20) # 20 | None saver.restore() #wait_for_duration(duration=1e-3) profiler.restore() ######################### solved = path is not None length = INF if path is None else len(path) cost = sum(get_distance_fn(robot, base_joints, weights=resolutions)(*pair) for pair in get_pairs(path)) print('Solved: {} | Length: {} | Cost: {:.3f} | Runtime: {:.3f} sec'.format( solved, length, cost, elapsed_time(start_time))) if path is None: disconnect() return iterate_path(robot, base_joints, path) disconnect()
def close_gripper_test(problem): joints = joints_from_names(problem.robot, PR2_GROUPS['left_gripper']) values = [get_min_limit(problem.robot, joint) for joint in joints] for _ in joint_controller_hold(problem.robot, joints, values): enable_gravity() step_simulation()
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 get_ik_fn(robot, fixed=[], teleport=False, num_attempts=10, self_collisions=True): # movable_joints = get_movable_joints(robot) torso_arm = get_torso_arm_joints(robot, ARM) arm_joints = joints_from_names(robot, get_arm_joint_names(ARM)) sample_fn = get_sample_fn(robot, torso_arm) def fn(body, pose, grasp): obstacles = [body] + fixed gripper_pose = end_effector_from_body(pose.pose, grasp.grasp_pose) approach_pose = approach_from_grasp(grasp.approach_pose, gripper_pose) draw_pose(get_tool_pose(robot, ARM), length=0.04) draw_pose(approach_pose, length=0.04) draw_pose(gripper_pose, length=0.04) # print(movable_joints) # print(torso_arm) # wait_for_interrupt() # TODO: gantry_x_joint # TODO: proper inverse reachability base_link = link_from_name(robot, IK_BASE_FRAMES[ARM]) base_pose = get_link_pose(robot, base_link) body_point_in_base = point_from_pose(multiply(invert(base_pose), pose.pose)) y_joint = joint_from_name(robot, '{}_gantry_y_joint'.format(prefix_from_arm(ARM))) initial_y = get_joint_position(robot, y_joint) ik_y = initial_y + SIGN_FROM_ARM[ARM]*body_point_in_base[1] set_joint_position(robot, y_joint, ik_y) for _ in range(num_attempts): if USE_IKFAST: q_approach = sample_tool_ik(robot, ARM, approach_pose) if q_approach is not None: set_joint_positions(robot, torso_arm, q_approach) else: set_joint_positions(robot, torso_arm, sample_fn()) # Random seed q_approach = inverse_kinematics(robot, grasp.link, approach_pose) if (q_approach is None) or any(pairwise_collision(robot, b) for b in obstacles): print('- ik for approaching fails!') continue conf = BodyConf(robot, joints=arm_joints) if USE_IKFAST: q_grasp = sample_tool_ik(robot, ARM, gripper_pose, closest_only=True) if q_grasp is not None: set_joint_positions(robot, torso_arm, q_grasp) else: conf.assign() q_grasp = inverse_kinematics(robot, grasp.link, gripper_pose) if (q_grasp is None) or any(pairwise_collision(robot, b) for b in obstacles): print('- ik for grasp fails!') continue if teleport: path = [q_approach, q_grasp] else: conf.assign() #direction, _ = grasp.approach_pose #path = workspace_trajectory(robot, grasp.link, point_from_pose(approach_pose), -direction, # quat_from_pose(approach_pose)) path = plan_direct_joint_motion(robot, torso_arm, q_grasp, obstacles=obstacles, self_collisions=self_collisions) if path is None: if DEBUG_FAILURE: print('Approach motion failed') continue command = Command([BodyPath(robot, path, joints=torso_arm), Attach(body, robot, grasp.link), BodyPath(robot, path[::-1], joints=torso_arm, attachments=[grasp])]) return (conf, command) # TODO: holding collisions return None return fn