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 extract_point2d(v): if isinstance(v, Conf): return v.values[:2] if isinstance(v, Pose): return point_from_pose(v.value)[:2] if v.stream == 'sample-pose': r, = v.values return point_from_pose(get_pose(r))[:2] if v.stream == 'inverse-kinematics': p, = v.values return extract_point2d(p) raise ValueError(v.stream)
def extract_point2d(v): if isinstance(v, Conf): return v.values[:2] if isinstance(v, Pose): return point_from_pose(v.value)[:2] if v.stream == 'p-sp': r, = v.inputs return point_from_pose(get_pose(r))[:2] if v.stream == 'q-ik': p, = v.inputs return extract_point2d(p) return ValueError(v.stream)
def test(o, p, bq): if o in task.rooms: return True target_xy = point_from_pose(p.value)[:2] base_xy = bq.values[:2] return range[0] <= get_length(np.array(target_xy) - base_xy) <= range[1]
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 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 gen(o, p): if o in task.rooms: # TODO: predicate instead return # 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: set_pose(o, p.value) # p.assign() bq = Conf(robot, base_joints, next(base_generator)) # bq = Pose(robot, get_pose(robot)) bq.assign() if any(pairwise_collision(robot, b) for b in fixed): yield None else: yield (bq, )
def fn(o, p, bq): set_pose(o, p.value) # p.assign() bq.assign() if o in task.rooms: waypoints = plan_scan_path(task.robot, tilt=ROOM_SCAN_TILT) set_group_conf(robot, 'head', waypoints[0]) path = plan_waypoints_joint_motion(robot, head_joints, waypoints[1:], obstacles=None, self_collisions=False) if path is None: return None ht = create_trajectory(robot, head_joints, path) hq = ht.path[0] else: target_point = point_from_pose(p.value) head_conf = inverse_visibility(robot, target_point) if head_conf is None: # TODO: test if visible return None hq = Conf(robot, head_joints, head_conf) ht = Trajectory([hq]) return (hq, ht)
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 __repr__(self): return '{}({},{})'.format(self.__class__.__name__, self.index, str(np.array(point_from_pose(self.value))))