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(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 opt_move_cost_fn(t): q1, q2 = t.values distance = get_distance(extract_point2d(q1), extract_point2d(q2)) return BASE_CONSTANT + distance / BASE_VELOCITY
def move_cost_fn(t): distance = t.distance( distance_fn=lambda q1, q2: get_distance(q1[:2], q2[:2])) return BASE_CONSTANT + distance / BASE_VELOCITY
def move_cost_fn(c): [t] = c.commands distance = t.distance( distance_fn=lambda q1, q2: get_distance(q1[:2], q2[:2])) #return BASE_CONSTANT + distance / BASE_VELOCITY return 1
def opt_move_cost_fn(t): q1, q2 = t.inputs distance = get_distance(extract_point2d(q1), extract_point2d(q2)) cost = BASE_CONSTANT + distance / BASE_VELOCITY return scale_cost(cost)
def distance_fn(q1, q2): distance = get_distance(q1.values[:2], q2.values[:2]) return BASE_CONSTANT + distance / BASE_VELOCITY
def fn(r, q1, q2): return get_distance(q1.values[:2], q2.values[:2])
def fn(r, q1, q2): xy_distance = get_distance(q1.values[:2], q2.values[:2]) return constant + coefficient * xy_distance