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],