def display_plan(problem, state, plan, time_step=0.01, sec_per_step=0.005): duration = compute_duration(plan) real_time = None if sec_per_step is None else (duration * sec_per_step / time_step) print('Duration: {} | Step size: {} | Real time: {}'.format(duration, time_step, real_time)) colors = {robot: COLOR_FROM_NAME[robot] for robot in problem.robots} for i, t in enumerate(inclusive_range(0, duration, time_step)): print('Step={} | t={}'.format(i, t)) for action in plan: name, args, start, duration = action if not (action.start <= t <= get_end(action)): continue if name == 'move': robot, conf1, traj, conf2 = args traj = [conf1.values, conf2.values] fraction = (t - action.start) / action.duration conf = get_value_at_time(traj, fraction) body = problem.get_body(robot) color = COLOR_FROM_NAME[robot] if colors[robot] != color: set_color(body, color, link_from_name(body, TOP_LINK)) colors[robot] = color set_base_conf(body, conf) elif name == 'recharge': robot, conf = args body = problem.get_body(robot) color = YELLOW if colors[robot] != color: set_color(body, color, link_from_name(body, TOP_LINK)) colors[robot] = color else: raise ValueError(name) if sec_per_step is None: wait_if_gui('Continue?') else: wait_for_duration(sec_per_step)
def get_ik_gen_fn(robot, brick_from_index, obstacle_from_name, max_attempts=10): movable_joints = get_movable_joints(robot) tool_link = link_from_name(robot, TOOL_NAME) disabled_collisions = { tuple( link_from_name(robot, link) for link in pair if has_link(robot, link)) for pair in DISABLED_COLLISIONS } sample_fn = get_sample_fn(robot, movable_joints) def gen_fn(index, pose, grasp): body = brick_from_index[index].body #world_pose = get_link_pose(robot, tool_link) #draw_pose(world_pose, length=0.04) #set_pose(body, multiply(world_pose, grasp.attach)) #draw_pose(multiply(pose.value, invert(grasp.attach)), length=0.04) #wait_for_interrupt() set_pose(body, pose.value) for _ in range(max_attempts): set_joint_positions(robot, movable_joints, sample_fn()) # Random seed attach_pose = multiply(pose.value, invert(grasp.attach)) attach_conf = inverse_kinematics(robot, tool_link, attach_pose) if attach_conf is None: continue approach_pose = multiply(attach_pose, ([0, 0, -0.1], unit_quat())) #approach_pose = multiply(pose.value, invert(grasp.approach)) approach_conf = inverse_kinematics(robot, tool_link, approach_pose) if approach_conf is None: continue # TODO: retreat path = plan_waypoints_joint_motion( robot, movable_joints, [approach_conf, attach_conf], obstacles=obstacle_from_name.values(), self_collisions=True, disabled_collisions=disabled_collisions) if path is None: continue #path = [approach_conf, attach_conf] attachment = Attachment(robot, tool_link, grasp.attach, body) traj = MotionTrajectory(robot, movable_joints, path, attachments=[attachment]) yield approach_conf, traj break return gen_fn
def get_disabled_collisions(robot): return { tuple( link_from_name(robot, link) for link in pair if has_link(robot, link)) for pair in DISABLED_COLLISIONS }
def gen(robot, body): link = link_from_name(robot, BASE_LINK) with BodySaver(robot): set_base_conf(robot, np.zeros(3)) robot_pose = get_link_pose(robot, link) robot_aabb = get_turtle_aabb(robot) # _, upper = robot_aabb # radius = upper[0] extent = get_aabb_extent(robot_aabb) radius = max(extent[:2]) / 2. #draw_aabb(robot_aabb) center, (diameter, height) = approximate_as_cylinder(body) distance = radius + diameter / 2. + epsilon _, _, z = get_point(body) # Assuming already placed stably for [scale] in unit_generator(d=1, use_halton=use_halton): #theta = PI # 0 | PI theta = random.uniform(*theta_interval) position = np.append(distance * unit_from_theta(theta=theta), [z]) # TODO: halton yaw = scale * 2 * PI - PI quat = quat_from_euler(Euler(yaw=yaw)) body_pose = (position, quat) robot_from_body = multiply(invert(robot_pose), body_pose) grasp = Pose(body, robot_from_body) # TODO: grasp instead of pose if draw: world_pose = multiply(get_link_pose(robot, link), grasp.value) set_pose(body, world_pose) handles = draw_pose(get_pose(body), length=1) wait_for_user() remove_handles(handles) #continue yield (grasp, )
def test_print(robot, node_points, elements): #elements = [elements[0]] elements = [elements[-1]] [element_body] = create_elements(node_points, elements) wait_for_interrupt() phi = 0 #grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=phi, yaw=theta)) # for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)] #grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=theta, yaw=phi)) # for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)] grasp_rotations = [sample_direction() for _ in range(25)] link = link_from_name(robot, TOOL_NAME) movable_joints = get_movable_joints(robot) sample_fn = get_sample_fn(robot, movable_joints) for grasp_rotation in grasp_rotations: n1, n2 = elements[0] length = np.linalg.norm(node_points[n2] - node_points[n1]) set_joint_positions(robot, movable_joints, sample_fn()) for t in np.linspace(-length / 2, length / 2, 10): #element_translation = Pose(point=Point(z=-t)) #grasp_pose = multiply(grasp_rotation, element_translation) #reverse = Pose(euler=Euler()) reverse = Pose(euler=Euler(roll=np.pi)) grasp_pose = get_grasp_pose(t, grasp_rotation, 0) grasp_pose = multiply(grasp_pose, reverse) element_pose = get_pose(element_body) link_pose = multiply(element_pose, invert(grasp_pose)) conf = inverse_kinematics(robot, link, link_pose) wait_for_interrupt()
def test_grasps(robot, node_points, elements): element = elements[-1] [element_body] = create_elements(node_points, [element]) phi = 0 link = link_from_name(robot, TOOL_NAME) link_pose = get_link_pose(robot, link) draw_pose(link_pose) #, parent=robot, parent_link=link) wait_for_interrupt() n1, n2 = element p1 = node_points[n1] p2 = node_points[n2] length = np.linalg.norm(p2 - p1) # Bottom of cylinder is p1, top is p2 print(element, p1, p2) for phi in np.linspace(0, np.pi, 10, endpoint=True): theta = np.pi / 4 for t in np.linspace(-length / 2, length / 2, 10): translation = Pose( point=Point(z=-t) ) # Want to be moving backwards because +z is out end effector direction = Pose(euler=Euler(roll=np.pi / 2 + theta, pitch=phi)) angle = Pose(euler=Euler(yaw=1.2)) grasp_pose = multiply(angle, direction, translation) element_pose = multiply(link_pose, grasp_pose) set_pose(element_body, element_pose) wait_for_interrupt()
def get_motion_fn(robot, brick_from_index, obstacle_from_name): movable_joints = get_movable_joints(robot) tool_link = link_from_name(robot, TOOL_NAME) disabled_collisions = { tuple( link_from_name(robot, link) for link in pair if has_link(robot, link)) for pair in DISABLED_COLLISIONS } def fn(conf1, conf2, fluents=[]): #path = [conf1, conf2] obstacles = list(obstacle_from_name.values()) attachments = [] for fact in fluents: if fact[0] == 'atpose': index, pose = fact[1:] body = brick_from_index[index].body set_pose(body, pose.value) obstacles.append(body) elif fact[0] == 'atgrasp': index, grasp = fact[1:] body = brick_from_index[index].body attachments.append( Attachment(robot, tool_link, grasp.attach, body)) else: raise NotImplementedError(fact[0]) set_joint_positions(robot, movable_joints, conf1) path = plan_joint_motion( robot, movable_joints, conf2, obstacles=obstacles, attachments=attachments, self_collisions=True, disabled_collisions=disabled_collisions, #weights=weights, resolutions=resolutions, restarts=5, iterations=50, smooth=100) if path is None: return None traj = MotionTrajectory(robot, movable_joints, path) return traj, return fn
def fn(robot, conf, body, grasp): conf.assign() link = link_from_name(robot, BASE_LINK) world_from_body = multiply(get_link_pose(robot, link), grasp.value) pose = Pose(body, world_from_body) pose.assign() if any(pairwise_collision(body, obst) for obst in problem.obstacles): return None return (pose, )
def __init__(self, robot, surface, detect=True, camera_frame=HEAD_LINK_NAME): self.robot = robot self.surface = surface self.camera_frame = camera_frame self.link = link_from_name(robot, self.camera_frame) self.detect = detect
def __init__(self, robot, body, camera_frame=HEAD_LINK_NAME, max_depth=MAX_KINECT_DISTANCE): self.robot = robot self.body = body self.camera_frame = camera_frame self.max_depth = max_depth self.link = link_from_name(robot, camera_frame)
def gen(rover, objective): base_joints = get_base_joints(rover) target_point = get_point(objective) base_generator = visible_base_generator(rover, target_point, base_range) lower_limits, upper_limits = get_custom_limits(rover, base_joints, custom_limits) attempts = 0 while True: if max_attempts <= attempts: attempts = 0 yield None attempts += 1 base_conf = next(base_generator) if not all_between(lower_limits, base_conf, upper_limits): continue bq = Conf(rover, base_joints, base_conf) bq.assign() if any(pairwise_collision(rover, b) for b in obstacles): continue link_pose = get_link_pose(rover, link_from_name(rover, KINECT_FRAME)) if use_cone: mesh, _ = get_detection_cone(rover, objective, camera_link=KINECT_FRAME, depth=max_range) if mesh is None: continue cone = create_mesh(mesh, color=(0, 1, 0, 0.5)) local_pose = Pose() else: distance = get_distance(point_from_pose(link_pose), target_point) if max_range < distance: continue cone = create_cylinder(radius=0.01, height=distance, color=(0, 0, 1, 0.5)) local_pose = Pose(Point(z=distance / 2.)) set_pose(cone, multiply(link_pose, local_pose)) # TODO: colors corresponding to scanned object if any( pairwise_collision(cone, b) for b in obstacles if b != objective and not is_placement(objective, b)): # TODO: ensure that this works for the rover remove_body(cone) continue if not reachable_test(rover, bq): continue print('Visibility attempts:', attempts) y = Ray(cone, rover, objective) yield Output(bq, y)
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 test_ik(robot, node_order, node_points): link = link_from_name(robot, TOOL_NAME) movable_joints = get_movable_joints(robot) sample_fn = get_sample_fn(robot, movable_joints) for n in node_order: point = node_points[n] set_joint_positions(robot, movable_joints, sample_fn()) conf = inverse_kinematics(robot, link, (point, None)) if conf is not None: set_joint_positions(robot, movable_joints, conf) continue link_point, link_quat = get_link_pose(robot, link) #print(point, link_point) #user_input('Continue?') wait_for_interrupt()
def compute_motions(robot, fixed_obstacles, element_bodies, initial_conf, trajectories): # TODO: can just plan to initial and then shortcut # TODO: backoff motion # TODO: reoptimize for the sequence that have the smallest movements given this # TODO: sample trajectories # TODO: more appropriate distance based on displacement/volume if trajectories is None: return None weights = np.array(JOINT_WEIGHTS) resolutions = np.divide(0.005 * np.ones(weights.shape), weights) movable_joints = get_movable_joints(robot) disabled_collisions = { tuple(link_from_name(robot, link) for link in pair) for pair in DISABLED_COLLISIONS } printed_elements = [] current_conf = initial_conf all_trajectories = [] for i, print_traj in enumerate(trajectories): set_joint_positions(robot, movable_joints, current_conf) goal_conf = print_traj.path[0] obstacles = fixed_obstacles + [ element_bodies[e] for e in printed_elements ] path = plan_joint_motion(robot, movable_joints, goal_conf, obstacles=obstacles, self_collisions=True, disabled_collisions=disabled_collisions, weights=weights, resolutions=resolutions, restarts=5, iterations=50, smooth=100) if path is None: print('Failed to find a path!') return None motion_traj = MotionTrajectory(robot, movable_joints, path) print('{}) {}'.format(i, motion_traj)) all_trajectories.append(motion_traj) current_conf = print_traj.path[-1] printed_elements.append(print_traj.element) all_trajectories.append(print_traj) # TODO: return to initial? return all_trajectories
def parse_fluents(robot, brick_from_index, fluents, obstacles): tool_link = link_from_name(robot, TOOL_NAME) attachments = [] for fact in fluents: if fact[0] == 'atpose': index, pose = fact[1:] body = brick_from_index[index].body set_pose(body, pose.value) obstacles.append(body) elif fact[0] == 'atgrasp': index, grasp = fact[1:] body = brick_from_index[index].body attachments.append(Attachment(robot, tool_link, grasp.attach, body)) else: raise NotImplementedError(fact[0]) return attachments
def get_target_point(conf): # TODO: use full body aabb robot = conf.body link = link_from_name(robot, 'torso_lift_link') #link = BASE_LINK # TODO: center of mass instead? # TODO: look such that cone bottom touches at bottom # TODO: the target isn't the center which causes it to drift with BodySaver(conf.body): conf.step() lower, upper = get_aabb(robot, link) center = np.average([lower, upper], axis=0) point = np.array(get_group_conf(conf.body, 'base')) #point[2] = upper[2] point[2] = center[2] #center, _ = get_center_extent(conf.body) return point
def compute_direction_path(robot, length, reverse, element_body, direction, collision_fn): step_size = 0.0025 # 0.005 #angle_step_size = np.pi / 128 angle_step_size = np.math.radians(0.25) angle_deltas = [-angle_step_size, 0, angle_step_size] #num_initial = 12 num_initial = 1 steps = np.append(np.arange(-length / 2, length / 2, step_size), [length / 2]) #print('Length: {} | Steps: {}'.format(length, len(steps))) #initial_angles = [wrap_angle(angle) for angle in np.linspace(0, 2*np.pi, num_initial, endpoint=False)] initial_angles = [ wrap_angle(angle) for angle in np.random.uniform(0, 2 * np.pi, num_initial) ] movable_joints = get_movable_joints(robot) sample_fn = get_sample_fn(robot, movable_joints) set_joint_positions(robot, movable_joints, sample_fn()) link = link_from_name(robot, TOOL_NAME) element_pose = get_pose(element_body) current_angle, current_conf = optimize_angle(robot, link, element_pose, steps[0], direction, reverse, initial_angles, collision_fn) if current_conf is None: return None # TODO: constrain maximum conf displacement # TODO: alternating minimization for just position and also orientation trajectory = [current_conf] for translation in steps[1:]: #set_joint_positions(robot, movable_joints, current_conf) initial_angles = [ wrap_angle(current_angle + delta) for delta in angle_deltas ] current_angle, current_conf = optimize_angle(robot, link, element_pose, translation, direction, reverse, initial_angles, collision_fn) if current_conf is None: return None trajectory.append(current_conf) return trajectory
def gen(robot, body): link = link_from_name(robot, BASE_LINK) with BodySaver(robot): set_base_conf(robot, np.zeros(3)) robot_pose = get_link_pose(robot, link) robot_aabb = get_turtle_aabb(robot) #draw_aabb(robot_aabb) lower, upper = robot_aabb center, (diameter, height) = approximate_as_cylinder(body) _, _, z = get_point(body) # Assuming already placed stably position = Point(x=upper[0] + diameter / 2., z=z) for [scale] in halton_generator(d=1): yaw = scale * 2 * np.pi - np.pi quat = quat_from_euler(Euler(yaw=yaw)) body_pose = (position, quat) robot_from_body = multiply(invert(robot_pose), body_pose) grasp = Pose(body, robot_from_body) yield (grasp, )
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 __init__(self, robot, surface): self.robot = robot self.surface = surface self.link = link_from_name(robot, HEAD_LINK_NAME)
def get_turtle_aabb(robot): return get_subtree_aabb(robot, link_from_name(robot, BASE_LINK))
def __init__(self, robot, arm, body): self.robot = robot self.arm = arm self.body = body self.link = link_from_name(self.robot, PR2_TOOL_FRAMES[self.arm])
def get_print_gen_fn(robot, obstacles, node_points, element_bodies, ground_nodes): max_attempts = 150 max_trajectories = 10 check_collisions = True # 50 doesn't seem to be enough movable_joints = get_movable_joints(robot) disabled_collisions = { tuple(link_from_name(robot, link) for link in pair) for pair in DISABLED_COLLISIONS } #element_neighbors = get_element_neighbors(element_bodies) node_neighbors = get_node_neighbors(element_bodies) incoming_supporters, _ = neighbors_from_orders( get_supported_orders(element_bodies, node_points)) # TODO: print on full sphere and just check for collisions with the printed element # TODO: can slide a component of the element down def gen_fn(node1, element, fluents=[]): # TODO: sample collisions while making the trajectory reverse = (node1 != element[0]) element_body = element_bodies[element] n1, n2 = reversed(element) if reverse else element delta = node_points[n2] - node_points[n1] # if delta[2] < 0: # continue length = np.linalg.norm(delta) # 5cm #supporters = {e for e in node_neighbors[n1] if element_supports(e, n1, node_points)} supporters = [] retrace_supporters(element, incoming_supporters, supporters) elements_order = [ e for e in element_bodies if (e != element) and (e not in supporters) ] bodies_order = [element_bodies[e] for e in elements_order] collision_fn = get_collision_fn( robot, movable_joints, obstacles + [element_bodies[e] for e in supporters], attachments=[], self_collisions=True, disabled_collisions=disabled_collisions, use_limits=True) trajectories = [] for num in irange(max_trajectories): for attempt in range(max_attempts): path = sample_print_path(robot, length, reverse, element_body, collision_fn) if path is None: continue if check_collisions: collisions = check_trajectory_collision( robot, path, bodies_order) colliding = { e for k, e in enumerate(elements_order) if (element != e) and collisions[k] } else: colliding = set() if (node_neighbors[n1] <= colliding) and not any( n in ground_nodes for n in element): continue print_traj = PrintTrajectory(robot, movable_joints, path, element, reverse, colliding) trajectories.append(print_traj) if print_traj not in trajectories: continue print('{}) {}->{} ({}) | {} | {} | {}'.format( num, n1, n2, len(supporters), attempt, len(trajectories), sorted(len(t.colliding) for t in trajectories))) yield print_traj, if not colliding: return else: print('{}) {}->{} ({}) | {} | Failure!'.format( num, len(supporters), n1, n2, max_attempts)) break return gen_fn
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) with LockRenderer(): draw_pose(unit_pose(), length=1, width=1) floor = create_floor() set_point(floor, Point(z=-EPSILON)) table1 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH / 2, height=TABLE_WIDTH / 2, top_color=TAN, cylinder=False) set_point(table1, Point(y=+0.5)) table2 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH / 2, height=TABLE_WIDTH / 2, top_color=TAN, cylinder=False) set_point(table2, Point(y=-0.5)) tables = [table1, table2] #set_euler(table1, Euler(yaw=np.pi/2)) with HideOutput(): # 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 = load_pybullet(robot_path, fixed_base=True) #dump_body(robot) block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED) block_z = stable_z(block1, table1) set_point(block1, Point(y=-0.5, z=block_z)) block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN) set_point(block2, Point(x=-0.25, y=-0.5, z=block_z)) block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE) set_point(block3, Point(x=-0.15, y=+0.5, z=block_z)) blocks = [block1, block2, block3] set_camera_pose(camera_point=Point(x=-1, z=block_z + 1), target_point=Point(z=block_z)) 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) grasps = get_side_grasps(block1, tool_pose=Pose(euler=Euler(yaw=np.pi / 2)), top_offset=0.02, grasp_length=0.03, under=False)[1:2] for grasp in grasps: gripper_pose = multiply(block_pose, invert(grasp)) set_pose(robot, multiply(gripper_pose, invert(base_from_tool))) wait_for_user() wait_for_user('Finish?') disconnect()
def __init__(self, robot, body): self.robot = robot self.body = body self.link = link_from_name(robot, HEAD_LINK_NAME)
def get_ik_gen_fn(robot, brick_from_index, obstacle_from_name, max_attempts=25): movable_joints = get_movable_joints(robot) tool_link = link_from_name(robot, TOOL_NAME) disabled_collisions = get_disabled_collisions(robot) sample_fn = get_sample_fn(robot, movable_joints) approach_distance = 0.1 #approach_distance = 0.0 approach_vector = approach_distance * np.array([0, 0, -1]) def gen_fn(index, pose, grasp): body = brick_from_index[index].body set_pose(body, pose.value) obstacles = list(obstacle_from_name.values()) # + [body] collision_fn = get_collision_fn( robot, movable_joints, obstacles=obstacles, attachments=[], self_collisions=SELF_COLLISIONS, disabled_collisions=disabled_collisions, custom_limits=get_custom_limits(robot)) attach_pose = multiply(pose.value, invert(grasp.attach)) approach_pose = multiply(attach_pose, (approach_vector, unit_quat())) # approach_pose = multiply(pose.value, invert(grasp.approach)) for _ in range(max_attempts): if IK_FAST: attach_conf = sample_tool_ik(robot, attach_pose) else: set_joint_positions(robot, movable_joints, sample_fn()) # Random seed attach_conf = inverse_kinematics(robot, tool_link, attach_pose) if (attach_conf is None) or collision_fn(attach_conf): continue set_joint_positions(robot, movable_joints, attach_conf) if IK_FAST: approach_conf = sample_tool_ik(robot, approach_pose, nearby_conf=attach_conf) else: approach_conf = inverse_kinematics(robot, tool_link, approach_pose) if (approach_conf is None) or collision_fn(approach_conf): continue set_joint_positions(robot, movable_joints, approach_conf) path = plan_direct_joint_motion( robot, movable_joints, attach_conf, obstacles=obstacles, self_collisions=SELF_COLLISIONS, disabled_collisions=disabled_collisions) if path is None: # TODO: retreat continue #path = [approach_conf, attach_conf] attachment = Attachment(robot, tool_link, grasp.attach, body) traj = MotionTrajectory(robot, movable_joints, path, attachments=[attachment]) yield approach_conf, traj break return gen_fn