def main(): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-viewer', action='store_true', help='enable viewer.') args = parser.parse_args() connect(use_gui=True) #ycb_path = os.path.join(DRAKE_PATH, DRAKE_YCB) #ycb_path = os.path.join(YCB_PATH, YCB_TEMPLATE.format('003_cracker_box')) #print(ycb_path) #load_pybullet(ycb_path) with LockRenderer(): draw_pose(unit_pose(), length=1, width=1) floor = create_floor() set_point(floor, Point(z=-EPSILON)) table = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2, top_color=TAN, cylinder=False) #set_euler(table, Euler(yaw=np.pi/2)) with HideOutput(False): # data_path = add_data_path() # robot_path = os.path.join(data_path, WSG_GRIPPER) robot_path = get_model_path(WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF #robot_path = get_file_path(__file__, 'mit_arch_suction_gripper/mit_arch_suction_gripper.urdf') robot = load_pybullet(robot_path, fixed_base=True) #dump_body(robot) #robot = create_cylinder(radius=0.5*BLOCK_SIDE, height=4*BLOCK_SIDE) # vacuum gripper block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED) block_z = stable_z(block1, table) set_point(block1, Point(z=block_z)) block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN) set_point(block2, Point(x=+0.25, z=block_z)) block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE) set_point(block3, Point(x=-0.15, z=block_z)) blocks = [block1, block2, block3] add_line(Point(x=-TABLE_WIDTH/2, z=block_z - BLOCK_SIDE/2 + EPSILON), Point(x=+TABLE_WIDTH/2, z=block_z - BLOCK_SIDE/2 + EPSILON), color=RED) set_camera_pose(camera_point=Point(y=-1, z=block_z+1), target_point=Point(z=block_z)) wait_for_user() block_pose = get_pose(block1) open_gripper(robot) tool_link = link_from_name(robot, 'tool_link') base_from_tool = get_relative_pose(robot, tool_link) #draw_pose(unit_pose(), parent=robot, parent_link=tool_link) y_grasp, x_grasp = get_top_grasps(block1, tool_pose=unit_pose(), grasp_length=0.03, under=False) grasp = y_grasp # fingers won't collide gripper_pose = multiply(block_pose, invert(grasp)) set_pose(robot, multiply(gripper_pose, invert(base_from_tool))) wait_for_user('Finish?') disconnect()
def draw_edges(edges): vertices = {v for edge in edges for v in edge} handles = [] for conf in vertices: handles.extend(draw_point(point_from_conf(conf.values), size=0.05)) for conf1, conf2 in edges: handles.append(add_line(point_from_conf(conf1.values), point_from_conf(conf2.values))) return handles
def fn(body, q1, q2): joints = get_base_joints(body) distance_fn = get_base_distance_fn(weights) extend_fn = get_extend_fn(body, joints, resolutions=resolutions) collision_fn = get_collision_fn(body, joints, problem.obstacles, attachments=[], self_collisions=False, disabled_collisions=set(), custom_limits=problem.custom_limits, max_distance=MAX_DISTANCE) # TODO: degree distance = distance_fn(q1.values, q2.values) if max_distance < distance: return None path = [q1.values] + list(extend_fn(q1.values, q2.values)) if any(map(collision_fn, path)): return None handles = add_line(point_from_conf(q1.values), point_from_conf(q2.values)) t = create_trajectory(body, joints, path) return (t,)
def test(r, t, b2, p2): if not problem.collisions: return True p2.assign() state = State() for _ in t.apply(state): state.assign() #for b1 in state.attachments: # if pairwise_collision(b1, b2): # return False if pairwise_collision(r, b2): set_renderer(True) color = get_visual_data(b2)[0].rgbaColor handles = add_line(point_from_conf(t.path[0].values, z=0.02), point_from_conf(t.path[-1].values, z=0.02), color=color) wait_for_user() set_renderer(False) return False return True
def display_trajectories(ground_nodes, trajectories, time_step=0.05): if trajectories is None: return connect(use_gui=True) floor, robot = load_world() wait_for_interrupt() movable_joints = get_movable_joints(robot) #element_bodies = dict(zip(elements, create_elements(node_points, elements))) #for body in element_bodies.values(): # set_color(body, (1, 0, 0, 0)) connected = set(ground_nodes) for trajectory in trajectories: if isinstance(trajectory, PrintTrajectory): print(trajectory, trajectory.n1 in connected, trajectory.n2 in connected, is_ground(trajectory.element, ground_nodes), len(trajectory.path)) connected.add(trajectory.n2) #wait_for_interrupt() #set_color(element_bodies[element], (1, 0, 0, 1)) last_point = None handles = [] for conf in trajectory.path: set_joint_positions(robot, movable_joints, conf) if isinstance(trajectory, PrintTrajectory): current_point = point_from_pose( get_link_pose(robot, link_from_name(robot, TOOL_NAME))) if last_point is not None: color = (0, 0, 1) if is_ground(trajectory.element, ground_nodes) else (1, 0, 0) handles.append( add_line(last_point, current_point, color=color)) last_point = current_point wait_for_duration(time_step) #wait_for_interrupt() #user_input('Finish?') wait_for_interrupt() disconnect()
def draw_element(node_points, element, color=(1, 0, 0)): n1, n2 = element p1 = node_points[n1] p2 = node_points[n2] return add_line(p1, p2, color=color[:3])