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)
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)