コード例 #1
0
    def closest_point(self, f_x, f_y, s_x, s_y, obs_bearing):
        '''
        Calculate the closest point on the line to the feature
        The feature is the point (probably not on the line)
        The line is defined by a point (state x and y) and direction (heading)
        This probably won't return a point that is behind the x-y-bearing.
        Input:
            f_x float (feature's x coordinate)
            f_y float (feature's y coordinate)
            s_x float (robot state's x)
            s_y float (robot state's y)
            obs_bearing float (robot state's heading)
        '''
        origin_to_feature = (f_x - s_x, f_y - s_y, 0.0,)
        line_parallel = unit((cos(obs_bearing), sin(obs_bearing), 0.0))

        # origin_to_feature dot line_parallel = magnitude of otf along line
        magmag = dot_product(origin_to_feature, line_parallel)
        
        if magmag < 0:
            return (s_x, s_y)
        
        scaled_line = scale(line_parallel, magmag)
        scaled_x = scaled_line[0]
        scaled_y = scaled_line[1]

        return (float(s_x + scaled_x), float(s_y + scaled_y))
コード例 #2
0
    def closest_point(self, f_x, f_y, s_x, s_y, obs_bearing):
        '''
        Calculate the closest point on the line to the feature
        The feature is the point (probably not on the line)
        The line is defined by a point (state x and y) and direction (heading)
        This probably won't return a point that is behind the x-y-bearing.
        Input:
            f_x float (feature's x coordinate)
            f_y float (feature's y coordinate)
            s_x float (robot state's x)
            s_y float (robot state's y)
            obs_bearing float (robot state's heading)
        '''
        origin_to_feature = (
            f_x - s_x,
            f_y - s_y,
            0.0,
        )
        line_parallel = unit((cos(obs_bearing), sin(obs_bearing), 0.0))

        # origin_to_feature dot line_parallel = magnitude of otf along line
        magmag = dot_product(origin_to_feature, line_parallel)

        if magmag < 0:
            return (s_x, s_y)

        scaled_line = scale(line_parallel, magmag)
        scaled_x = scaled_line[0]
        scaled_y = scaled_line[1]

        return (float(s_x + scaled_x), float(s_y + scaled_y))
コード例 #3
0
    def generate_plan(self, start, goal):
        # debug = None
        debug = rospy.loginfo
        if debug is not None:
            debug('potential generate plane\n'+str(goal))
        deck = deque()
        deck.append(start)

        next_ = deepcopy(start)

        distance = (math.pow(next_.position.x-goal.position.x, 2) 
            + math.pow(next_.position.y-goal.position.y, 2))

        if distance < .01:
            debug('the start distance is very small')
            return []

        debug('dist: %f' % (distance,))

        step_size = 0.2

        count = max(10, int(1.0/step_size))

        while(distance > .01 and count >= 0):
            debug('start calculating obs_force')
            obs_force = self.calc_potential(next_)
            debug('end calculating obs_force')
            goal_force = self.goal_force(next_, goal)
            
            total_force = addv(obs_force, goal_force)
            total_force = unit(total_force)
            debug('%s\n%s\n%s' % (obs_force, goal_force, total_force))

            dx = total_force[0]*step_size
            dy = total_force[1]*step_size

            debug('d: %f , %f' % (dx, dy,))
            new_pose = Pose()
            new_pose.position.x = next_.position.x+dx
            new_pose.position.y = next_.position.y+dy
            new_pose.orientation = heading_to_quaternion(math.atan2(dy, dx))

            deck.append(new_pose)

            next_ = deepcopy(new_pose)

            distance = (math.pow(next_.position.x-goal.position.x, 2) 
                + math.pow(next_.position.y-goal.position.y, 2))
            debug('dist: %f' % (distance,))
            count += -1
        if (distance > .01):
            debug('count break')
        else:
            deck.append(goal)

        return list(deck)
コード例 #4
0
    def goal_force(self, pose, goal):
        dx = goal.position.x - pose.position.x
        dy = goal.position.y - pose.position.y

        weight = 5.0

        farce = (dx, dy, 0)
        farce = unit(farce)
        farce = scale(farce, weight)
        return farce
コード例 #5
0
ファイル: driver.py プロジェクト: cwrucutter/drive_stack
    def off_axis_error(self, location, goal):
        """
        calc error normal to axis defined by the goal position and direction

        input: two nav_msgs.msg.Odometry, current best location estimate and
         goal
        output: double distance along the axis

        axis is defined by a vector from the unit circle aligned with the goal
         heading
        relative position is the vector from the goal x, y to the location x, y

        distance is defined by subtracting the parallel vector from the total
         relative position vector

        example use:
        see calc_errors above
        """
        relative_position_x = (location.pose.pose.position.x -
            goal.pose.pose.position.x)
        relative_position_y = (location.pose.pose.position.y -
            goal.pose.pose.position.y)
        
        # relative position of the best estimate position and the goal
        # vector points from the goal to the location
        ## relative_position = (relative_position_x, relative_position_y, 0.0)

        goal_heading = quaternion_to_heading(goal.pose.pose.orientation)
        goal_vector_x = math.cos(goal_heading)
        goal_vector_y = math.sin(goal_heading)

        # vector in the direction of the goal heading, axis of desired motion
        goal_vector = (goal_vector_x, goal_vector_y, 0.0)

        along_axis_error = self.along_axis_error(location, goal)

        along_axis_vec = scale(unit(goal_vector), along_axis_error)

        new_rel_x = relative_position_x - along_axis_vec[0]
        new_rel_y = relative_position_y - along_axis_vec[1]
        
        new_rel_vec = (new_rel_x, new_rel_y, 0.0)

        error_magnitude = math.sqrt(new_rel_x*new_rel_x + 
            new_rel_y*new_rel_y)

        if error_magnitude < .0001:
            return 0.0

        if cross_product(goal_vector, new_rel_vec)[2] >= 0.0:
            return error_magnitude
        else:
            return -error_magnitude
コード例 #6
0
def construct_problem_mixed(spline_template,
                            observed_accel_timestamps,
                            observed_accel_orientations,
                            observed_accel_readings,
                            observed_frame_timestamps,
                            observed_frame_orientations,
                            observed_features,
                            imu_to_camera=np.eye(3),
                            camera_matrix=np.eye(3),
                            feature_tolerance=1e-2,
                            gravity_magnitude=9.8,
                            max_bias_magnitude=.1):
    if len(observed_features) < 5:
        raise InsufficientObservationsError()

    # Sanity checks
    assert isinstance(spline_template, spline.SplineTemplate)
    assert len(observed_accel_orientations) == len(observed_accel_readings)
    assert len(observed_accel_timestamps) == len(observed_accel_readings)
    assert len(observed_frame_timestamps) == len(observed_frame_orientations)
    assert all(0 <= f.frame_id < len(observed_frame_timestamps) for f in observed_features)
    assert np.ndim(observed_accel_timestamps) == 1
    assert np.ndim(observed_frame_timestamps) == 1

    # Compute offsets
    position_offset = 0
    position_len = spline_template.control_size
    gravity_offset = position_offset + position_len
    accel_bias_offset = gravity_offset + 3
    structure_offset = accel_bias_offset + 3
    track_ids = set(f.track_id for f in observed_features)

    num_aux_vars = 1  # one extra variable representing the objective
    num_frames = len(observed_frame_timestamps)
    num_tracks = max(track_ids) + 1
    num_vars = structure_offset + num_tracks * 3 + num_aux_vars

    # Make sure each track has at least one observation
    counts_by_frame = np.zeros(num_frames, int)
    counts_by_track = np.zeros(num_tracks, int)
    for f in observed_features:
        counts_by_frame[f.frame_id] += 1
        counts_by_track[f.track_id] += 1

    if not np.all(counts_by_frame > 0):
        raise InsufficientObservationsError(
            'These frames had zero features: ' + ','.join(map(str, np.flatnonzero(counts_by_frame == 0))))
    if not np.all(counts_by_track > 0):
        raise InsufficientObservationsError(
            'These tracks had zero features: ' + ','.join(map(str, np.flatnonzero(counts_by_track == 0))))

    # Track IDs should be exactly 0..n-1
    assert all(track_id < num_tracks for track_id in track_ids)

    # Initialize the problem
    objective = utils.unit(num_vars-1, num_vars)   # the last variable is the objective we minimize
    problem = socp.SocpProblem(objective)

    # Construct accel constraints
    print 'Constructing constraints for %d accel readings...' % len(observed_accel_readings)
    accel_coefficients = spline_template.coefficients_d2(observed_accel_timestamps)
    accel_j_blocks = []
    accel_r_blocks = []
    for r, a, c in zip(observed_accel_orientations, observed_accel_readings, accel_coefficients):
        amat = spline.diagify(c, 3)
        j = np.zeros((3, num_vars))
        j[:, :position_len] = np.dot(r, amat)
        j[:, gravity_offset:gravity_offset+3] = r
        j[:, accel_bias_offset:accel_bias_offset+3] = np.eye(3)
        accel_j_blocks.append(j)
        accel_r_blocks.append(a)

    # Form the least squares objective || J*x + r ||^2
    accel_j = np.vstack(accel_j_blocks)
    accel_r = np.hstack(accel_r_blocks)

    # Form the quadratic objective: x' J' J x + b' x + c <= objective  ("objective" is the variable we minimize)
    accel_c = np.dot(accel_r, accel_r)
    accel_b = -2. * np.dot(accel_j.T, accel_r)
    accel_b[-1] = -1.

    # Convert to an SOCP objective
    problem.add_constraint(*soc_constraint_from_quadratic_constraint(accel_j, accel_b, accel_c))

    # Construct gravity constraints
    a_gravity = np.zeros((3, num_vars))
    a_gravity[:, gravity_offset:gravity_offset+3] = np.eye(3)
    d_gravity = gravity_magnitude
    problem.add_constraint(a=a_gravity, d=d_gravity)

    # Construct accel bias constraints
    a_bias = np.zeros((3, num_vars))
    a_bias[:, accel_bias_offset:accel_bias_offset+3] = np.eye(3)
    d_bias = max_bias_magnitude
    problem.add_constraint(a=a_bias, d=d_bias)

    # Construct vision constraints
    print 'Constructing constraints for %d features...' % len(observed_features)
    pos_coefficients = spline_template.coefficients(observed_frame_timestamps)
    pos_multidim_coefs = [spline.diagify(x, 3) for x in pos_coefficients]
    for feature in observed_features:
        r = observed_frame_orientations[feature.frame_id]
        pmat = pos_multidim_coefs[feature.frame_id]

        point_offset = structure_offset + feature.track_id*3
        assert point_offset + 3 <= num_vars, 'track id was %d, num vars was %d' % (feature.track_id, num_vars)

        k_rc_r = np.dot(camera_matrix, np.dot(imu_to_camera, r))
        ymat = np.zeros((3, num_vars))
        ymat[:, :position_len] = -np.dot(k_rc_r, pmat)
        ymat[:, point_offset:point_offset+3] = k_rc_r

        a_feature = ymat[:2] - np.outer(feature.position, ymat[2])
        c_feature = ymat[2] * feature_tolerance

        problem.add_constraint(a=a_feature, c=c_feature)

    return problem