def quaternion_that_rotates_axes_frame(
source_xyz_axes: Tuple[Vector3r, Vector3r, Vector3r],
target_xyz_axes: Tuple[Vector3r, Vector3r, Vector3r],
assume_normalized: bool = False, # warn if it isn't
) -> Quaternionr:
""" Returns the quaternion that rotates vectors from the `source` coordinate system to the `target` axes frame. """
if not assume_normalized:
assert all(np.isclose(_.get_length(), 1) for _ in source_xyz_axes + target_xyz_axes)
# ref.: https://math.stackexchange.com/a/909245
i, j, k = source_xyz_axes
a, b, c = target_xyz_axes
def quaternion_from_vector(v: Vector3r) -> Quaternionr:
return Quaternionr(v.x_val, v.y_val, v.z_val, w_val=0)
qx = quaternion_from_vector(a) * quaternion_from_vector(i)
qy = quaternion_from_vector(b) * quaternion_from_vector(j)
qz = quaternion_from_vector(c) * quaternion_from_vector(k)
rx = qx.x_val + qy.x_val + qz.x_val
ry = qx.y_val + qy.y_val + qz.y_val
rz = qx.z_val + qy.z_val + qz.z_val
rw = qx.w_val + qy.w_val + qz.w_val
rotation = Quaternionr(-rx, -ry, -rz, w_val=(1 - rw))
length = rotation.get_length()
assert not np.isclose(length, 0)
return rotation / length # normalize
def next_quaternion(self, ):
if self.oriind >= self.SmoothCount: # sample a new orientation
rand_ori = self.random_quaternion()
new_ori = rand_ori * self.orientation
new_qtn = Quaternionr(new_ori.x, new_ori.y, new_ori.z, new_ori.w)
(pitch, roll, yaw) = to_eularian_angles(new_qtn)
# print ' %.2f, %.2f, %.2f' %(pitch, roll, yaw )
pitch = np.clip(pitch, self.MinPitch, self.MaxPitch)
roll = np.clip(roll, self.MinRoll, self.MaxRoll)
yaw = np.clip(yaw, self.MinYaw, self.MaxYaw)
# print ' %.2f, %.2f, %.2f' %(pitch, roll, yaw )
new_qtn_clip = to_quaternion(pitch, roll, yaw)
new_ori_clip = Quaternionpy(new_qtn_clip.w_val, new_qtn_clip.x_val,
new_qtn_clip.y_val, new_qtn_clip.z_val)
qtnlist = Quaternionpy.intermediates(self.orientation,
new_ori_clip,
self.SmoothCount - 2,
include_endpoints=True)
self.orientation = new_ori_clip
self.orilist = list(qtnlist)
self.oriind = 1
# print "sampled new", new_ori, ', after clip', self.orientation #, 'list', self.orilist
next_qtn = self.orilist[self.oriind]
self.oriind += 1
# print " return next", next_qtn
return Quaternionr(next_qtn.x, next_qtn.y, next_qtn.z, next_qtn.w)
def convert_2_pose(a):
"""
a: A 7-element array.
"""
return Pose(Vector3r(a[0], a[1], a[2]),
Quaternionr(a[3], a[4], a[5], a[6]))
def quaternion_from_rotation_axis_angle(axis: Vector3r, angle: float, is_degrees: bool = False) -> Quaternionr:
if is_degrees:
angle = np.deg2rad(angle)
half_angle = angle / 2
axis /= axis.get_length() # normalize
axis *= np.sin(half_angle)
return Quaternionr(axis.x_val, axis.y_val, axis.z_val, w_val=np.cos(half_angle))
def quaternion_from_two_vectors(a: Vector3r, b: Vector3r) -> Quaternionr:
""" What rotation (around the intersection of the two vectors) we need to rotate `a` to `b`? """
# ref.: https://gitlab.com/libeigen/eigen/-/blob/master/Eigen/src/Geometry/Quaternion.h (FromTwoVectors)
v0 = a / a.get_length()
v1 = b / b.get_length()
c = v1.dot(v0)
assert c > -1 # FIXME handle the case when the vectors are nearly opposites
s = np.sqrt((1 + c) * 2)
axis = v0.cross(v1) / s
return Quaternionr(axis.x_val, axis.y_val, axis.z_val, w_val=(s / 2))
def vector_rotated_by_quaternion(v: Vector3r, q: Quaternionr) -> Vector3r:
q /= q.get_length() # normalize
# Extract the vector and scalar parts of q:
u, s = Vector3r(q.x_val, q.y_val, q.z_val), q.w_val
# NOTE the results from these two methods are the same up to 7 decimal places.
# ref.: https://gamedev.stackexchange.com/questions/28395/rotating-vector3-by-a-quaternion
# return u * (2 * u.dot(v)) + v * (s * s - u.dot(u)) + u.cross(v) * (2 * s)
# ref.: https://gitlab.com/libeigen/eigen/-/blob/master/Eigen/src/Geometry/Quaternion.h (_transformVector)
uv = u.cross(v)
uv += uv
return v + uv * s + u.cross(uv)
def __init__(
self,
time_stamp: int,
pos_x: float, pos_y: float, pos_z: float,
q_w: float, q_x: float, q_y: float, q_z: float,
image_file: str,
):
""" Represents an item from AirSim's recording file (i.e. a row in `airsim_rec.txt`).
Reference: https://github.com/microsoft/AirSim/blob/master/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp
"""
# assert os.path.isfile(image_file), image_file
self.time_stamp = time_stamp
self.image_file = image_file
self.position = Vector3r(pos_x, pos_y, pos_z)
self.orientation = Quaternionr(q_x, q_y, q_z, q_w)
def setRandomPose(client, center, offset_range, rotation_range):
random_vector = np.random.random_sample((2, 1))
unit_vector = random_vector / np.linalg.norm(random_vector)
displacement_vector = (offset_range[1] - offset_range[0]) * (
np.random.random_sample()) * unit_vector + offset_range[0]
displacement_vector = displacement_vector.reshape(-1, )
position = Vector3r(center.position.x_val + displacement_vector[0],
center.position.y_val + displacement_vector[1],
center.position.z_val)
rotation = (rotation_range[1] - rotation_range[0]
) * np.random.random_sample() + rotation_range[
0] #2*np.sin(2*np.pi*np.random.random_sample())
orientation = Quaternionr(z_val=rotation)
pose = Pose(position_val=position, orientation_val=orientation)
client.simSetObjectPose("pallet", pose, True)
return pose, displacement_vector, rotation
def quaternion_from_rotator(pitch: float, yaw: float, roll: float, is_degrees: bool = False) -> Quaternionr:
if is_degrees:
pitch = np.deg2rad(np.fmod(pitch, 360))
yaw = np.deg2rad(np.fmod(yaw, 360))
roll = np.deg2rad(np.fmod(roll, 360))
pitch *= 0.5
yaw *= 0.5
roll *= 0.5
sp, cp = np.sin(pitch), np.cos(pitch)
sy, cy = np.sin(yaw), np.cos(yaw)
sr, cr = np.sin(roll), np.cos(roll)
x = +(cr * sp * sy) - (sr * cp * cy)
y = -(cr * sp * cy) - (sr * cp * sy)
z = +(cr * cp * sy) - (sr * sp * cy)
w = +(cr * cp * cy) + (sr * sp * sy)
# NOTE calling airsim.to_quaternion(pitch, roll, yaw) is equivalent to
# passing this return to AirSimNedTransform.quaternion_from_uu_to_ned.
return Quaternionr(x, y, z, w_val=w)
def capture_LIDAR_depth(self, p, q):
"""
p: A 3-element NumPy array, the position.
q: A 4-element NumPy array, quaternion, w is the last element.
"""
faces = [0, 1, 2, -1] # Front, right, back, left.
depthList = []
q0 = Quaternionr(q[0], q[1], q[2], q[3])
for face in faces:
# Compose a AirSim Pose object.
yaw = np.pi / 2 * face
yq = to_quaternion(0, 0, yaw)
# q = to_quaternion( e[0], e[1], e[2] )
q1 = yq * q0
pose = Pose(Vector3r(p[0], p[1], p[2]), q1)
# Change the vehicle pose.
self.set_vehicle_pose(pose)
# Get the image.
for i in range(self.maxTrial):
depth, _ = self.get_depth_campos()
if depth is None:
print("Fail for trail %d on face %d. " % (i, face))
continue
if (depth is None):
raise Exception("Could not get depth image for face %d. " %
(face))
# Get a valid depth.
depthList.append(depth)
return depthList
def setPose(center, z_rotation):
pose = Pose(Vector3r(center[0], center[1], center[2]),
Quaternionr(z_val=z_rotation))
return pose
def quaternion_from_vector(v: Vector3r) -> Quaternionr:
return Quaternionr(v.x_val, v.y_val, v.z_val, w_val=0)
def quaternion_from_ned_to_uu(q: Quaternionr):
return Quaternionr(-q.x_val, -q.y_val, q.z_val, w_val=q.w_val)
def quaternion_flip_y_axis(q: Quaternionr) -> Quaternionr: return Quaternionr(-q.x_val, q.y_val, -q.z_val, w_val=q.w_val) def quaternion_flip_x_axis(q: Quaternionr) -> Quaternionr: return Quaternionr(q.x_val, -q.y_val, -q.z_val, w_val=q.w_val)
def vector_rotated_by_quaternion_reversed(v: Vector3r, q: Quaternionr) -> Vector3r:
# NOTE we could have used q.conjugate(), if we assumed q was a unit quaternion
return vector_rotated_by_quaternion(v, q.inverse())
def quaternion_flip_x_axis(q: Quaternionr) -> Quaternionr: return Quaternionr(q.x_val, -q.y_val, -q.z_val, w_val=q.w_val) def quaternion_that_rotates_axes_frame(
def quaternion_that_rotates_orientation(
from_orientation: Quaternionr,
to_orientation: Quaternionr,
) -> Quaternionr:
""" Returns the quaternion that rotates `from_orientation` into `to_orientation`. """
return to_orientation * from_orientation.inverse()
def pose_from_meshroom_to_airsim(meshroom_pose):
assert len(meshroom_pose.center) == 3 and len(
meshroom_pose.rotation) == 9, meshroom_pose
xyzw = MeshroomTransform.rotation(meshroom_pose.rotation,
as_xyzw_quaternion=True)
return Pose(Vector3r(*meshroom_pose.center), Quaternionr(*xyzw))
X = Vector3r(1, 0, 0)
Y = Vector3r(0, 1, 0)
Z = Vector3r(0, 0, 1)
RGB = (Rgba.Red, Rgba.Green, Rgba.Blue)
CMY = (Rgba.Cyan, Rgba.Magenta, Rgba.Yellow)
LOOK_AT_TARGET = data_config.Ned.Cidadela_Statue
TEST_POSE = Pose(
Vector3r(x_val=-13.793405532836914,
y_val=2.257002115249634,
z_val=-6.261967182159424),
Quaternionr(
x_val=-0.020065542310476303,
y_val=-0.14743053913116455,
z_val=-0.02142292633652687,
w_val=0.9886367917060852,
),
)
def plot_xyz_axis(
client: airsim.MultirotorClient,
x_axis: Vector3r,
y_axis: Vector3r,
z_axis: Vector3r,
origin: Vector3r,
colors=RGB,
thickness=2.5,
) -> None:
client.simPlotArrows([origin], [origin + x_axis],
