def roll_and_pitch_mapping_a_to_b(a, b):
    """
    Computes the roll and pitch needed to map a point on another, on a sphere.

    The two input points are given by their cartesian coordinates (x, y, z). The
    computed angles phi and psi are such that the rotation obtained by composing
    an elemental rotation of phi about the x-axis with an elemental rotation of
    psi about the new y-axis maps a to b.

    Args:
        a, b: two array-like objects of length 3.

    Returns:
        phi, psi: the two roll and pitch angles such that R_x(phi)R_y(psi)a = b.
    """
    # checks that the input 3d points are on the same sphere
    np.testing.assert_allclose(np.linalg.norm(a), np.linalg.norm(b))

    # compute the roll angle phi
    x = utils.solve_acos_plus_bsin_plus_c(b[1], b[2], -a[1])

    # compute the pitch angle psi
    bb = np.dot(np.linalg.inv(utils.elemental_rotation_3d('x', x)), b)
    y = np.arctan2(bb[0], bb[2]) - np.arctan2(a[0], a[2])

    # check that the composition of R_x(phi) and R_y(psi) maps a to b
    np.testing.assert_allclose(np.dot(utils.rotation_3d('xyz', x, y, 0), a), b)
    return x, y
    def locdir_at(self, phi, theta, psi, row, col, alt=0, geo=True):
        """
        Given the satellite attitude, computes lon, lat of point row, col, alt.

        Args:
            phi, theta, psi: attitude angles of the satellite camera
            row, col: pixel coordinates in the image plane
            alt: altitude of the 3d point above the Earth surface
            geo: boolean flag telling whether to return geographic
                coordinates or Cartesian coordinates.

        Returns:
            geographic coordinates (lon, lat), or Cartesian coordinates
            (x, y, z). Cartesian coordinates are computed with respect to the
            orbital frame.
        """
        # first compute coordinates of the 3-space point in the orbital frame
        orbital_to_camera = utils.rotation_3d('xyz', phi, theta, psi)
        v = np.dot(orbital_to_camera, self.instrument.sight_vector(col))
        c = np.array([0, 0, self.orbit.radius])
        r = PhysicalConstants.earth_radius + alt
        p = utils.intersect_line_and_sphere(v, c, r)
        if not geo:
            return p

        # then convert them to lon, lat, using t to get the satellite position
        if p is None:
            return p
        else:
            t = row * self.instrument.dwell_time
            mat_t_ol = self.rotational_to_orbital(t)
            return utils.lon_lat(np.dot(mat_t_ol, -c + p))
    def inertial_to_orbital(self, pso):
        """
        Computes the change of basis matrix from inertial to orbital frame.

        If X is the coordinate vector of a 3-space vector expressed in the
        inertial frame, and X' is the coordinate vector of the same 3-space
        vector expressed in the orbital frame, then the matrix P computed by
        this function is such that X = PX'.

        The change of basis is a simple rotation computed from the 3 angles
        lon, i and pso. The orbital plane precession is neglected.

        Args:
           pso: satellite angular position, in degrees.
        """
        # first rotation about z axis. It sends the x axis to reference lon.
        lon = self.lon * np.pi/180

        # second rotation about the new x axis, to send the z axis in the
        # orbital plane. The angle between the xz plane (x stands for the new x
        # axis) and the orbital plane is i - pi/2
        inc = self.i - np.pi/2

        # third rotation about the new y axis. It sends the new x axis to
        # the Earth-Satellite axis. The angle about y is given by -pso.
        # Additional rotation of -pi/2 about the same axis, such that the new z
        # axis points towards the Earth center.
        pos = -pso * np.pi/180 - np.pi/2
        return utils.rotation_3d('zxy', lon, inc, pos)
    def locdir_at(self, phi, theta, psi, row, col, alt=0, geo=True):
        """
        Given the satellite attitude, computes lon, lat of point row, col, alt.

        Args:
            phi, theta, psi: attitude angles of the satellite camera
            row, col: pixel coordinates in the image plane
            alt: altitude of the 3d point above the Earth surface
            geo: boolean flag telling whether to return geographic
                coordinates or Cartesian coordinates.

        Returns:
            geographic coordinates (lon, lat), or Cartesian coordinates
            (x, y, z). Cartesian coordinates are computed with respect to the
            orbital frame.
        """
        # first compute coordinates of the 3-space point in the orbital frame
        orbital_to_camera = utils.rotation_3d('xyz', phi, theta, psi)
        v = np.dot(orbital_to_camera, self.instrument.sight_vector(col))
        c = np.array([0, 0, self.orbit.radius])
        r = PhysicalConstants.earth_radius + alt
        p = utils.intersect_line_and_sphere(v, c, r)
        if not geo:
            return p

        # then convert them to lon, lat, using t to get the satellite position
        if p is None:
            return p
        else:
            t = row * self.instrument.dwell_time
            mat_t_ol = self.rotational_to_orbital(t)
            return utils.lon_lat(np.dot(mat_t_ol, -c + p))
def roll_and_pitch_mapping_a_to_b(a, b):
    """
    Computes the roll and pitch needed to map a point on another, on a sphere.

    The two input points are given by their cartesian coordinates (x, y, z). The
    computed angles phi and psi are such that the rotation obtained by composing
    an elemental rotation of phi about the x-axis with an elemental rotation of
    psi about the new y-axis maps a to b.

    Args:
        a, b: two array-like objects of length 3.

    Returns:
        phi, psi: the two roll and pitch angles such that R_x(phi)R_y(psi)a = b.
    """
    # checks that the input 3d points are on the same sphere
    np.testing.assert_allclose(np.linalg.norm(a), np.linalg.norm(b))

    # compute the roll angle phi
    x = utils.solve_acos_plus_bsin_plus_c(b[1], b[2], -a[1])

    # compute the pitch angle psi
    bb = np.dot(np.linalg.inv(utils.elemental_rotation_3d('x', x)), b)
    y = np.arctan2(bb[0], bb[2]) - np.arctan2(a[0], a[2])

    # check that the composition of R_x(phi) and R_y(psi) maps a to b
    np.testing.assert_allclose(np.dot(utils.rotation_3d('xyz', x, y, 0), a), b)
    return x, y
    def inertial_to_orbital(self, pso):
        """
        Computes the change of basis matrix from inertial to orbital frame.

        If X is the coordinate vector of a 3-space vector expressed in the
        inertial frame, and X' is the coordinate vector of the same 3-space
        vector expressed in the orbital frame, then the matrix P computed by
        this function is such that X = PX'.

        The change of basis is a simple rotation computed from the 3 angles
        lon, i and pso. The orbital plane precession is neglected.

        Args:
           pso: satellite angular position, in degrees.
        """
        # first rotation about z axis. It sends the x axis to reference lon.
        lon = self.lon * np.pi / 180

        # second rotation about the new x axis, to send the z axis in the
        # orbital plane. The angle between the xz plane (x stands for the new x
        # axis) and the orbital plane is i - pi/2
        inc = self.i - np.pi / 2

        # third rotation about the new y axis. It sends the new x axis to
        # the Earth-Satellite axis. The angle about y is given by -pso.
        # Additional rotation of -pi/2 about the same axis, such that the new z
        # axis points towards the Earth center.
        pos = -pso * np.pi / 180 - np.pi / 2
        return utils.rotation_3d('zxy', lon, inc, pos)
Exemplo n.º 7
0
    def compute_approach_direction(self, mesh, grasp_vertices):

        ## initalizing stuff ##
        nb_directions_to_test = 6
        normal_scale = 0.01
        plane_normal = normalize(grasp_vertices[0] - grasp_vertices[1])

        midpoint = (grasp_vertices[0] + grasp_vertices[1]) / 2

        ## generating a certain number of approach directions ##
        theta = np.pi / nb_directions_to_test
        rot_mat = rotation_3d(-plane_normal, theta)

        horizontal_direction = normalize(
            np.cross(plane_normal, np.array([0, 0, 1])))
        directions_to_test = [horizontal_direction]  #these are vectors
        approach_directions = [
            np.array(
                [midpoint, midpoint + horizontal_direction * normal_scale])
        ]  #these are two points for visualization

        for i in range(nb_directions_to_test - 1):
            directions_to_test.append(
                normalize(np.matmul(rot_mat, directions_to_test[-1])))
            approach_directions.append(
                np.array([
                    midpoint, midpoint + directions_to_test[-1] * normal_scale
                ]))

        ## computing the palm position for each approach direction ##
        palm_positions = []
        for i in range(nb_directions_to_test):
            palm_positions.append(midpoint +
                                  finger_length * directions_to_test[i])

        directions_to_test = [
            directions_to_test[3], directions_to_test[2],
            directions_to_test[4], directions_to_test[1],
            directions_to_test[5], directions_to_test[0]
        ]
        palm_positions = [
            palm_positions[3], palm_positions[2], palm_positions[4],
            palm_positions[1], palm_positions[5], palm_positions[0]
        ]

        ## checking if some approach direction is valid ##
        for i in range(nb_directions_to_test):
            if len(
                    trimesh.intersections.mesh_plane(mesh,
                                                     directions_to_test[i],
                                                     palm_positions[i])) == 0:
                # it means the palm won't bump with part
                return directions_to_test[i]

        # it means all approach directions will bump with part
        return -1
    def attitude_from_point_and_speed(self, m, v):
        """
        Derives the attitude from a targeted point and a pushbroom direction.

        The roll, pitch, yaw angles transforming the orbital frame into the
        camera frame are computed. The camera frame is defined by a sight
        direction (given by the ground point m) and the normal vector to the
        projection of the pushbroom array on the ground (given by v). Rotations
        have to be applied in this order: roll, then pitch, then yaw.

        Args:
            m: point on Earth, given by its coordinates in the orbital frame
            v: movement of the pushbroom array, projected on the ground, given
                in the orbital frame. It has to be a unit vector.

        Returns:
            phi, theta, psi: attitude angles in radians
        """
        # computation of phi and theta is straightforward (do a drawing)
        phi = -np.arctan(m[1] / m[2])
        theta = np.arcsin(m[0] / np.linalg.norm(m))

        # computation of psi
        # normal vector to the ground in m
        n = -np.array([0, 0, self.orbit.radius]) + m
        n = n / np.linalg.norm(n)

        # vector orthogonal to v, on the ground
        w = np.cross(n, v)

        rot = utils.rotation_3d('xyz', phi, theta, 0)
        x = np.dot(rot, np.array([1, 0, 0]))
        y = np.dot(rot, np.array([0, 1, 0]))
        xx = np.cross(y, n)  # projection of x on the tangent plane
        yy = np.cross(x, n)  # projection of y on the tangent plane

        if np.dot(w, yy) == 0:
            psi = np.pi/2
        else:
            psi = np.arctan(np.dot(w, xx) / np.dot(w, yy))

        sp = np.sin(psi)
        cp = np.cos(psi)

		# correction
        sgcosgammar = np.dot(v, sp*yy - cp*xx)
        if sgcosgammar < 0:
            if psi < 0:
                psi += np.pi
            else:
                psi -= np.pi

        return phi, theta, psi
    def attitude_from_point_and_speed(self, m, v):
        """
        Derives the attitude from a targeted point and a pushbroom direction.

        The roll, pitch, yaw angles transforming the orbital frame into the
        camera frame are computed. The camera frame is defined by a sight
        direction (given by the ground point m) and the normal vector to the
        projection of the pushbroom array on the ground (given by v). Rotations
        have to be applied in this order: roll, then pitch, then yaw.

        Args:
            m: point on Earth, given by its coordinates in the orbital frame
            v: movement of the pushbroom array, projected on the ground, given
                in the orbital frame. It has to be a unit vector.

        Returns:
            phi, theta, psi: attitude angles in radians
        """
        # computation of phi and theta is straightforward (do a drawing)
        phi = -np.arctan(m[1] / m[2])
        theta = np.arcsin(m[0] / np.linalg.norm(m))

        # computation of psi
        # normal vector to the ground in m
        n = -np.array([0, 0, self.orbit.radius]) + m
        n = n / np.linalg.norm(n)

        # vector orthogonal to v, on the ground
        w = np.cross(n, v)

        rot = utils.rotation_3d('xyz', phi, theta, 0)
        x = np.dot(rot, np.array([1, 0, 0]))
        y = np.dot(rot, np.array([0, 1, 0]))
        xx = np.cross(y, n)  # projection of x on the tangent plane
        yy = np.cross(x, n)  # projection of y on the tangent plane

        if np.dot(w, yy) == 0:
            psi = np.pi / 2
        else:
            psi = np.arctan(np.dot(w, xx) / np.dot(w, yy))

        sp = np.sin(psi)
        cp = np.cos(psi)

        # correction
        sgcosgammar = np.dot(v, sp * yy - cp * xx)
        if sgcosgammar < 0:
            if psi < 0:
                psi += np.pi
            else:
                psi -= np.pi

        return phi, theta, psi
    def guidance_algorithm(self, psi_x, psi_y, cap, dt, alt=0, slow_factor=1):
        """
        Computes attitude samples used to estimate the attitude functions.

        Args:
            psi_x: sight direction angle about the x axis of orbital frame, degrees
            psi_y: sight direction angle about the y axis of orbital frame, degrees
            cap: direction of the pushbroom array movement, on the ground, in
                degrees. North is 0, Sud is 180 and East is 90.
            dt: temporal sampling step
            alt (default 0): altitude used to compute the attitude samples
            slow_factor (default 1): slow down factor applied to the speed of the
                projection of the pushbroom array on the ground.

        Returns:
            t: 1d np.array containing the dates at which the attitudes are
                sampled.
            phi, theta, psi: three 1d np.array containing the roll, pitch and
                yaw samples.
        """
        # number of samples used to interpolate the attitude angles
        n = int(self.duration / dt) + 1
        theta = np.zeros(n)
        phi = np.zeros(n)
        psi = np.zeros(n)

        # dates at which the attitude angles are sampled
        t = np.linspace(0, (n-1)*dt, n)

        # p is the point on Earth spotted by the satellite, in orbital frame
        p = self.orbit.target_point_orbital_frame(psi_x, psi_y, alt)

        for k in range(n):
            # direction of pushbroom motion, on the ground, in orbital frame
            v = self.pushbroom_direction_on_the_ground(cap, p, t[k])

            # attitude
            phi[k], theta[k], psi[k] = self.attitude_from_point_and_speed(p, v)
            orb_to_cam = utils.rotation_3d('xyz', phi[k], theta[k], psi[k])

            # speed of the pushbroom motion, on the ground
            norm = self.pixel_projection_width(orb_to_cam)
            norm /= (self.instrument.dwell_time * slow_factor)

            # update the targeted point
            p += v * norm * dt
            p -= np.array([0, 0, self.orbit.radius])
            p = np.dot(self.rotational_to_orbital(t[k]), p)
            p = np.linalg.solve(self.rotational_to_orbital(t[k]+dt), p)
            p += np.array([0, 0, self.orbit.radius])

        return t, phi, theta, psi
    def guidance_algorithm(self, psi_x, psi_y, cap, dt, alt=0, slow_factor=1):
        """
        Computes attitude samples used to estimate the attitude functions.

        Args:
            psi_x: sight direction angle about the x axis of orbital frame, degrees
            psi_y: sight direction angle about the y axis of orbital frame, degrees
            cap: direction of the pushbroom array movement, on the ground, in
                degrees. North is 0, Sud is 180 and East is 90.
            dt: temporal sampling step
            alt (default 0): altitude used to compute the attitude samples
            slow_factor (default 1): slow down factor applied to the speed of the
                projection of the pushbroom array on the ground.

        Returns:
            t: 1d np.array containing the dates at which the attitudes are
                sampled.
            phi, theta, psi: three 1d np.array containing the roll, pitch and
                yaw samples.
        """
        # number of samples used to interpolate the attitude angles
        n = int(self.duration / dt) + 1
        theta = np.zeros(n)
        phi = np.zeros(n)
        psi = np.zeros(n)

        # dates at which the attitude angles are sampled
        t = np.linspace(0, (n - 1) * dt, n)

        # p is the point on Earth spotted by the satellite, in orbital frame
        p = self.orbit.target_point_orbital_frame(psi_x, psi_y, alt)

        for k in range(n):
            # direction of pushbroom motion, on the ground, in orbital frame
            v = self.pushbroom_direction_on_the_ground(cap, p, t[k])

            # attitude
            phi[k], theta[k], psi[k] = self.attitude_from_point_and_speed(p, v)
            orb_to_cam = utils.rotation_3d('xyz', phi[k], theta[k], psi[k])

            # speed of the pushbroom motion, on the ground
            norm = self.pixel_projection_width(orb_to_cam)
            norm /= (self.instrument.dwell_time * slow_factor)

            # update the targeted point
            p += v * norm * dt
            p -= np.array([0, 0, self.orbit.radius])
            p = np.dot(self.rotational_to_orbital(t[k]), p)
            p = np.linalg.solve(self.rotational_to_orbital(t[k] + dt), p)
            p += np.array([0, 0, self.orbit.radius])

        return t, phi, theta, psi
    def orbital_to_camera(self, t):
        """
        Computes the change of basis matrix from orbital to camera frame.

        This change of basis is a rotation determined by roll, pitch and yaw
        angles, which are computed from t.

        Args:
            t: time in seconds from the beginning of the acquisition

        Returns:
            numpy 3x3 array representing the change of basis matrix
        """
        phi = np.polyval(self.poly_phi, t)
        theta = np.polyval(self.poly_theta, t)
        psi = np.polyval(self.poly_psi, t)
        return utils.rotation_3d('xyz', phi, theta, psi)
    def orbital_to_camera(self, t):
        """
        Computes the change of basis matrix from orbital to camera frame.

        This change of basis is a rotation determined by roll, pitch and yaw
        angles, which are computed from t.

        Args:
            t: time in seconds from the beginning of the acquisition

        Returns:
            numpy 3x3 array representing the change of basis matrix
        """
        phi = np.polyval(self.poly_phi, t)
        theta = np.polyval(self.poly_theta, t)
        psi = np.polyval(self.poly_psi, t)
        return utils.rotation_3d('xyz', phi, theta, psi)