def optimize_angle(robot, link, element_pose, translation, direction, reverse, initial_angles, collision_fn, max_error=1e-2): movable_joints = get_movable_joints(robot) initial_conf = get_joint_positions(robot, movable_joints) best_error, best_angle, best_conf = max_error, None, None for i, angle in enumerate(initial_angles): grasp_pose = get_grasp_pose(translation, direction, angle, reverse) target_pose = multiply(element_pose, invert(grasp_pose)) conf = inverse_kinematics(robot, link, target_pose) # if conf is None: # continue #if pairwise_collision(robot, robot): conf = get_joint_positions(robot, movable_joints) if not collision_fn(conf): link_pose = get_link_pose(robot, link) error = get_distance(point_from_pose(target_pose), point_from_pose(link_pose)) if error < best_error: # TODO: error a function of direction as well best_error, best_angle, best_conf = error, angle, conf # wait_for_interrupt() if i != len(initial_angles) - 1: set_joint_positions(robot, movable_joints, initial_conf) #print(best_error, translation, direction, best_angle) if best_conf is not None: set_joint_positions(robot, movable_joints, best_conf) #wait_for_interrupt() return best_angle, best_conf
def gen(o, p): # default_conf = arm_conf(a, g.carry) # joints = get_arm_joints(robot, a) # TODO: check collisions with fixed links target_point = point_from_pose(p.value) base_generator = visible_base_generator(robot, target_point, base_range) while True: for _ in range(max_attempts): set_pose(o, p.value) base_conf = next(base_generator) #set_base_values(robot, base_conf) set_joint_positions(robot, base_joints, base_conf) if any(pairwise_collision(robot, b) for b in fixed): continue head_conf = inverse_visibility(robot, target_point) if head_conf is None: # TODO: test if visible continue #bq = Pose(robot, get_pose(robot)) bq = Conf(robot, base_joints, base_conf) hq = Conf(robot, head_joints, head_conf) yield (bq, hq) break else: yield None
def fn(conf1, conf2, fluents=[]): if teleport is True: path = [conf1, conf2] traj = MotionTrajectory(robot, movable_joints, path) return traj, obstacles = list(obstacle_from_name.values()) attachments = parse_fluents(robot, brick_from_index, fluents, obstacles) set_joint_positions(robot, movable_joints, conf1) path = plan_joint_motion( robot, movable_joints, conf2, obstacles=obstacles, attachments=attachments, self_collisions=SELF_COLLISIONS, 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,
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 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,
def test_confs(robot, num_samples=10): 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(num_samples): print('Iteration:', i) conf = sample_fn() set_joint_positions(robot, joints, conf) wait_for_interrupt()
def check_trajectory_collision(robot, trajectory, bodies): # TODO: each new addition makes collision checking more expensive #offset = 4 movable_joints = get_movable_joints(robot) #for q in trajectory[offset:-offset]: collisions = [False for _ in range(len(bodies))] # TODO: batch collision detection for q in trajectory: set_joint_positions(robot, movable_joints, q) for i, body in enumerate(bodies): if not collisions[i]: collisions[i] |= pairwise_collision(robot, body) return collisions
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 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 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 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
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
def iterate(self): for conf in self.path[1:]: set_joint_positions(self.robot, self.joints, conf) yield
def set_base_conf(robot, conf): set_joint_positions(robot, get_base_joints(robot), conf)