symcad.core.Rotation
1#!/usr/bin/env python3 2# Copyright (C) 2022, Will Hedgecock 3# 4# This program is free software: you can redistribute it and/or modify 5# it under the terms of the GNU General Public License as published by 6# the Free Software Foundation, either version 3 of the License, or 7# (at your option) any later version. 8# 9# This program is distributed in the hope that it will be useful, 10# but WITHOUT ANY WARRANTY; without even the implied warranty of 11# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12# GNU General Public License for more details. 13# 14# You should have received a copy of the GNU General Public License 15# along with this program. If not, see <http://www.gnu.org/licenses/>. 16 17from __future__ import annotations 18from typing import List, Optional, Tuple, Union 19from sympy import Expr, Symbol 20from copy import deepcopy 21from operator import mul 22import math, sympy 23 24Quaternion = Tuple[Union[float, Expr], Union[float, Expr], Union[float, Expr], Union[float, Expr]] 25 26class Rotation(object): 27 """Represents a simple right-handed rotation assuming the nautical and aeronautical convention 28 of intrinsic `yaw, pitch, roll` rotation order. 29 """ 30 31 # Public attributes ---------------------------------------------------------------------------- 32 33 name: str 34 """Unique, identifying name of the `Rotation` instance.""" 35 36 roll: Union[float, Expr] 37 """Intrinsic roll angle in radians.""" 38 39 pitch: Union[float, Expr] 40 """Intrinsic pitch angle in radians.""" 41 42 yaw: Union[float, Expr] 43 """Intrinsic yaw angle in radians.""" 44 45 46 # Constructor and factory methods -------------------------------------------------------------- 47 48 def __init__(self, identifier: str, **kwargs) -> None: 49 """Initializes a `Rotation` instance, where `identifier` uniquely identifies the instance, 50 and `**kwargs` may contain the keywords `roll`, `pitch`, or `yaw` to specify a concrete 51 value for each component of the rotation. If any of these keywords are missing, the 52 corresponding angle of rotation will be assumed to be `0`. 53 54 Alternately, a `Rotation` instance can be created from: 55 56 - A quaternion, 57 - An explicit set of yaw, pitch, and roll angles, or 58 - A 3D rotation matrix. 59 60 A quaternion or rotation matrix is retrievable from any `Rotation` instance, regardless of 61 how it was created. 62 63 All rotations utilize a right-handed coordinate system and follow the convention of using 64 intrinsic rotations in the order `yaw, pitch, roll`. In other words, all rotations move in 65 the counter-clockwise direction when viewed from a positive axis looking toward origin, 66 first around the z-axis, then the y-axis, and finally around the x-axis. 67 """ 68 super().__init__() 69 self.name = identifier 70 self.roll = kwargs.get('roll', 0.0) 71 self.pitch = kwargs.get('pitch', 0.0) 72 self.yaw = kwargs.get('yaw', 0.0) 73 74 75 @classmethod 76 def from_angles(cls, identifier: str, 77 roll_rad: Union[float, Expr, None], 78 pitch_rad: Union[float, Expr, None], 79 yaw_rad: Union[float, Expr, None]) -> Rotation: 80 """Constructs a `Rotation` object with the specified roll, pitch, and yaw angles. 81 82 Parameters 83 ---------- 84 roll_rad : `Union[float, sympy.Expr, None]` 85 Desired intrinsic roll angle in radians. If `None` is specified, the angle will be 86 treated as a symbol. 87 pitch_rad : `Union[float, sympy.Expr, None]` 88 Desired intrinsic pitch angle in radians. If `None` is specified, the angle will be 89 treated as a symbol. 90 yaw_rad : `Union[float, sympy.Expr, None]` 91 Desired intrinsic yaw angle in radians. If `None` is specified, the angle will be 92 treated as a symbol. 93 94 Returns 95 ------- 96 `Rotation` 97 The newly created Rotation instance. 98 """ 99 roll = roll_rad if roll_rad is not None else Symbol(identifier + '_roll') 100 pitch = pitch_rad if pitch_rad is not None else Symbol(identifier + '_pitch') 101 yaw = yaw_rad if yaw_rad is not None else Symbol(identifier + '_yaw') 102 return cls(identifier, roll=roll, pitch=pitch, yaw=yaw) 103 104 105 @classmethod 106 def from_quaternion(cls, identifier: str, quaternion: Quaternion) -> Rotation: 107 """Constructs a `Rotation` object from the specified quaternion. 108 109 The ordering of the quaternion is expected to be: `q0, q1, q2, q3` or 110 equivalently `qw, qx, qy, qz`. 111 112 Parameters 113 ---------- 114 quaternion : `Quaternion` 115 Desired quaternion from which to construct a Rotation object. 116 117 Returns 118 ------- 119 `Rotation` 120 The newly created Rotation instance. 121 """ 122 123 # Roll calculation 124 sinroll_cospitch = 2.0*(quaternion[0]*quaternion[1] + quaternion[2]*quaternion[3]) 125 cosroll_cospitch = 1.0 - 2.0*(quaternion[1]*quaternion[1] + quaternion[2]*quaternion[2]) 126 roll = sympy.atan2(sinroll_cospitch, cosroll_cospitch) 127 128 # Pitch calculation 129 sinpitch = 2.0*(quaternion[0]*quaternion[2] - quaternion[3]*quaternion[1]) 130 pitch = math.copysign(0.5 * math.pi, sinpitch) if not isinstance(sinpitch, Expr) \ 131 and abs(sinpitch) >= 1.0 else \ 132 sympy.asin(sinpitch) 133 134 # Yaw calculation 135 sinyaw_cospitch = 2.0*(quaternion[0]*quaternion[3] + quaternion[1]*quaternion[2]) 136 cosyaw_cospitch = 1.0 - 2.0*(quaternion[2]*quaternion[2] + quaternion[3]*quaternion[3]) 137 yaw = sympy.atan2(sinyaw_cospitch, cosyaw_cospitch) 138 return cls(identifier, roll=roll, pitch=pitch, yaw=yaw) 139 140 141 @classmethod 142 def from_rotation_matrix(cls, identifier: str, rotation_matrix: List[List[float]]) -> Rotation: 143 """Constructs a `Rotation` object from the specified rotation matrix. 144 145 Parameters 146 ---------- 147 rotation_matrix : `List[List[float]]` 148 Desired 3x3 rotation matrix from which to construct a Rotation object. 149 150 Returns 151 ------- 152 `Rotation` 153 The newly created Rotation instance. 154 """ 155 roll = sympy.atan2(rotation_matrix[2][1], rotation_matrix[2][2]) 156 pitch = -sympy.asin(rotation_matrix[2][0]) 157 yaw = sympy.atan2(rotation_matrix[1][0], rotation_matrix[0][0]) 158 return cls(identifier, roll=roll, pitch=pitch, yaw=yaw) 159 160 161 # Built-in method implementations -------------------------------------------------------------- 162 163 def __repr__(self) -> str: 164 roll = self.roll * 180.0 / math.pi 165 pitch = self.pitch * 180.0 / math.pi 166 yaw = self.yaw * 180.0 / math.pi 167 return 'roll = {}\u00b0, pitch = {}\u00b0, yaw = {}\u00b0'.format(roll, pitch, yaw) 168 169 def __eq__(self, other: Rotation) -> bool: 170 return (self.roll == other.roll) and (self.pitch == other.pitch) and (self.yaw == other.yaw) 171 172 def __copy__(self): 173 copy = self.__class__.__new__(self.__class__) 174 copy.__dict__.update(self.__dict__) 175 return copy 176 177 def __deepcopy__(self, memo): 178 copy = self.__class__.__new__(self.__class__) 179 memo[id(self)] = copy 180 for key, val in self.__dict__.items(): 181 setattr(copy, key, deepcopy(val, memo)) 182 return copy 183 184 185 # Public methods ------------------------------------------------------------------------------- 186 187 def clone(self) -> Rotation: 188 """Returns an exact clone of this `Rotation` instance.""" 189 return deepcopy(self) 190 191 192 def copy_from(self, other: Rotation) -> Rotation: 193 """Copies the rotation angles from another `Rotation` instance into this one. 194 195 The name of this instance will not be changed or overwritten. 196 197 Parameters 198 ---------- 199 other : `Rotation` 200 A Rotation object whose contents should be copied into this instance. 201 202 Returns 203 ------- 204 self : `Rotation` 205 The Rotation instance being manipulated. 206 """ 207 self.roll = other.roll 208 self.pitch = other.pitch 209 self.yaw = other.yaw 210 return self 211 212 213 def set(self, *, roll_deg: Union[float, Expr, None], 214 pitch_deg: Union[float, Expr, None], 215 yaw_deg: Union[float, Expr, None]) -> Rotation: 216 """Sets the roll, pitch, and yaw angles to the specified values. 217 218 Parameters 219 ---------- 220 roll_deg : `Union[float, sympy.Expr, None]` 221 Desired intrinsic roll angle in degrees. If `None` is specified, the angle will be 222 treated as a symbol. 223 pitch_deg : `Union[float, sympy.Expr, None]` 224 Desired intrinsic pitch angle in degrees. If `None` is specified, the angle will be 225 treated as a symbol. 226 yaw_deg : `Union[float, sympy.Expr, None]` 227 Desired intrinsic yaw angle in degrees. If `None` is specified, the angle will be 228 treated as a symbol. 229 230 Returns 231 ------- 232 self : `Rotation` 233 The Rotation instance being manipulated. 234 """ 235 self.roll = Symbol(self.name + '_roll') if roll_deg is None else \ 236 roll_deg * math.pi / 180.0 237 self.pitch = Symbol(self.name + '_pitch') if pitch_deg is None else \ 238 pitch_deg * math.pi / 180.0 239 self.yaw = Symbol(self.name + '_yaw') if yaw_deg is None else \ 240 yaw_deg * math.pi / 180.0 241 return self 242 243 244 def clear(self) -> Rotation: 245 """Clears all rotation angles to `0` degrees. 246 247 Returns 248 ------- 249 self : `Rotation` 250 The Rotation instance being manipulated. 251 """ 252 self.roll = self.pitch = self.yaw = 0.0 253 return self 254 255 256 def rotate_point(self, rotation_center: Tuple[float, float, float], 257 point: Tuple[float, float, float]) -> Tuple[float, float, float]: 258 """Rotates a `point` around its `rotation_center` according to the current `Rotation` 259 instance properties. 260 261 Parameters 262 ---------- 263 rotation_center : `Tuple[float, float, float]` 264 Cartestion coordinate around which to carry out the specified rotation. 265 point : `Tuple[float, float, float]` 266 The Cartesian coordinates of the point to be rotated. 267 268 Returns 269 ------- 270 `Tuple[float, float, float]` 271 The final Cartesian coordinates of the rotated point. 272 """ 273 R = self.get_rotation_matrix() 274 centered_point = [point[i] - rotation_center[i] for i in range(3)] 275 rotated_point = [sum(map(mul, R[i], centered_point)) for i in range(3)] 276 return tuple([rotation_center[i] + rotated_point[i] for i in range(3)]) 277 278 279 def get_quaternion(self) -> Quaternion: 280 """Returns a quaternion representing the `Rotation` object. 281 282 The ordering of the quaternion will be: `q0, q1, q2, q3` or 283 equivalently `qw, qx, qy, qz`. 284 285 Returns 286 ------- 287 `Quaternion` 288 A quaternion representing this Rotation object. 289 """ 290 half_roll = 0.5 * self.roll 291 half_pitch = 0.5 * self.pitch 292 half_yaw = 0.5 * self.yaw 293 s = sympy.cos(half_roll) * sympy.cos(half_pitch) * sympy.cos(half_yaw) \ 294 + sympy.sin(half_roll) * sympy.sin(half_pitch) * sympy.sin(half_yaw) 295 q1 = sympy.sin(half_roll) * sympy.cos(half_pitch) * sympy.cos(half_yaw) \ 296 - sympy.cos(half_roll) * sympy.sin(half_pitch) * sympy.sin(half_yaw) 297 q2 = sympy.cos(half_roll) * sympy.sin(half_pitch) * sympy.cos(half_yaw) \ 298 + sympy.sin(half_roll) * sympy.cos(half_pitch) * sympy.sin(half_yaw) 299 q3 = sympy.cos(half_roll) * sympy.cos(half_pitch) * sympy.sin(half_yaw) \ 300 - sympy.sin(half_roll) * sympy.sin(half_pitch) * sympy.cos(half_yaw) 301 return s, q1, q2, q3 302 303 304 def get_rotation_matrix_row(self, row_index: int) -> List[List[float]]: 305 """Returns a single row from the 3D rotation matrix representing the `Rotation` object. 306 307 Parameters 308 ---------- 309 row_index : `int` 310 Index of the rotation matrix row to return between [0, 2]. 311 312 Returns 313 ------- 314 `List[float]` 315 A 3-element list representing a single row from the rotation matrix for this Rotation object. 316 """ 317 if row_index == 0: 318 rotation_matrix0 = sympy.cos(self.pitch)*sympy.cos(self.yaw) 319 rotation_matrix1 = sympy.sin(self.roll)*sympy.sin(self.pitch)*sympy.cos(self.yaw) \ 320 - sympy.sin(self.yaw)*sympy.cos(self.roll) 321 rotation_matrix2 = sympy.sin(self.roll)*sympy.sin(self.yaw) \ 322 + sympy.sin(self.pitch)*sympy.cos(self.roll)*sympy.cos(self.yaw) 323 elif row_index == 1: 324 rotation_matrix0 = sympy.sin(self.yaw)*sympy.cos(self.pitch) 325 rotation_matrix1 = sympy.sin(self.roll)*sympy.sin(self.pitch)*sympy.sin(self.yaw) \ 326 + sympy.cos(self.roll)*sympy.cos(self.yaw) 327 rotation_matrix2 = sympy.sin(self.pitch)*sympy.sin(self.yaw)*sympy.cos(self.roll) \ 328 - sympy.sin(self.roll)*sympy.cos(self.yaw) 329 elif row_index == 2: 330 rotation_matrix0 = -sympy.sin(self.pitch) 331 rotation_matrix1 = sympy.sin(self.roll)*sympy.cos(self.pitch) 332 rotation_matrix2 = sympy.cos(self.roll)*sympy.cos(self.pitch) 333 else: 334 raise RuntimeError('Invalid row_index parameter ({})...must be between 0 and 2'.format(row_index)) 335 return [rotation_matrix0, rotation_matrix1, rotation_matrix2] 336 337 338 def get_rotation_matrix(self) -> List[List[float]]: 339 """Returns a 3D rotation matrix representing the `Rotation` object. 340 341 Returns 342 ------- 343 `List[List[float]]` 344 A 3x3 rotation matrix representing this Rotation object. 345 """ 346 rotation_matrix00 = sympy.cos(self.pitch)*sympy.cos(self.yaw) 347 rotation_matrix01 = sympy.sin(self.roll)*sympy.sin(self.pitch)*sympy.cos(self.yaw) \ 348 - sympy.sin(self.yaw)*sympy.cos(self.roll) 349 rotation_matrix02 = sympy.sin(self.roll)*sympy.sin(self.yaw) \ 350 + sympy.sin(self.pitch)*sympy.cos(self.roll)*sympy.cos(self.yaw) 351 rotation_matrix10 = sympy.sin(self.yaw)*sympy.cos(self.pitch) 352 rotation_matrix11 = sympy.sin(self.roll)*sympy.sin(self.pitch)*sympy.sin(self.yaw) \ 353 + sympy.cos(self.roll)*sympy.cos(self.yaw) 354 rotation_matrix12 = sympy.sin(self.pitch)*sympy.sin(self.yaw)*sympy.cos(self.roll) \ 355 - sympy.sin(self.roll)*sympy.cos(self.yaw) 356 rotation_matrix20 = -sympy.sin(self.pitch) 357 rotation_matrix21 = sympy.sin(self.roll)*sympy.cos(self.pitch) 358 rotation_matrix22 = sympy.cos(self.roll)*sympy.cos(self.pitch) 359 return [[rotation_matrix00, rotation_matrix01, rotation_matrix02], 360 [rotation_matrix10, rotation_matrix11, rotation_matrix12], 361 [rotation_matrix20, rotation_matrix21, rotation_matrix22]] 362 363 364 def as_tuple(self) -> Tuple[float, float, float]: 365 """Returns the current yaw-pitch-roll angles as a `tuple` in degrees.""" 366 return (self.yaw * 180.0 / math.pi, 367 self.pitch * 180.0 / math.pi, 368 self.roll * 180.0 / math.pi)
Quaternion =
typing.Tuple[typing.Union[float, sympy.core.expr.Expr], typing.Union[float, sympy.core.expr.Expr], typing.Union[float, sympy.core.expr.Expr], typing.Union[float, sympy.core.expr.Expr]]
class
Rotation:
27class Rotation(object): 28 """Represents a simple right-handed rotation assuming the nautical and aeronautical convention 29 of intrinsic `yaw, pitch, roll` rotation order. 30 """ 31 32 # Public attributes ---------------------------------------------------------------------------- 33 34 name: str 35 """Unique, identifying name of the `Rotation` instance.""" 36 37 roll: Union[float, Expr] 38 """Intrinsic roll angle in radians.""" 39 40 pitch: Union[float, Expr] 41 """Intrinsic pitch angle in radians.""" 42 43 yaw: Union[float, Expr] 44 """Intrinsic yaw angle in radians.""" 45 46 47 # Constructor and factory methods -------------------------------------------------------------- 48 49 def __init__(self, identifier: str, **kwargs) -> None: 50 """Initializes a `Rotation` instance, where `identifier` uniquely identifies the instance, 51 and `**kwargs` may contain the keywords `roll`, `pitch`, or `yaw` to specify a concrete 52 value for each component of the rotation. If any of these keywords are missing, the 53 corresponding angle of rotation will be assumed to be `0`. 54 55 Alternately, a `Rotation` instance can be created from: 56 57 - A quaternion, 58 - An explicit set of yaw, pitch, and roll angles, or 59 - A 3D rotation matrix. 60 61 A quaternion or rotation matrix is retrievable from any `Rotation` instance, regardless of 62 how it was created. 63 64 All rotations utilize a right-handed coordinate system and follow the convention of using 65 intrinsic rotations in the order `yaw, pitch, roll`. In other words, all rotations move in 66 the counter-clockwise direction when viewed from a positive axis looking toward origin, 67 first around the z-axis, then the y-axis, and finally around the x-axis. 68 """ 69 super().__init__() 70 self.name = identifier 71 self.roll = kwargs.get('roll', 0.0) 72 self.pitch = kwargs.get('pitch', 0.0) 73 self.yaw = kwargs.get('yaw', 0.0) 74 75 76 @classmethod 77 def from_angles(cls, identifier: str, 78 roll_rad: Union[float, Expr, None], 79 pitch_rad: Union[float, Expr, None], 80 yaw_rad: Union[float, Expr, None]) -> Rotation: 81 """Constructs a `Rotation` object with the specified roll, pitch, and yaw angles. 82 83 Parameters 84 ---------- 85 roll_rad : `Union[float, sympy.Expr, None]` 86 Desired intrinsic roll angle in radians. If `None` is specified, the angle will be 87 treated as a symbol. 88 pitch_rad : `Union[float, sympy.Expr, None]` 89 Desired intrinsic pitch angle in radians. If `None` is specified, the angle will be 90 treated as a symbol. 91 yaw_rad : `Union[float, sympy.Expr, None]` 92 Desired intrinsic yaw angle in radians. If `None` is specified, the angle will be 93 treated as a symbol. 94 95 Returns 96 ------- 97 `Rotation` 98 The newly created Rotation instance. 99 """ 100 roll = roll_rad if roll_rad is not None else Symbol(identifier + '_roll') 101 pitch = pitch_rad if pitch_rad is not None else Symbol(identifier + '_pitch') 102 yaw = yaw_rad if yaw_rad is not None else Symbol(identifier + '_yaw') 103 return cls(identifier, roll=roll, pitch=pitch, yaw=yaw) 104 105 106 @classmethod 107 def from_quaternion(cls, identifier: str, quaternion: Quaternion) -> Rotation: 108 """Constructs a `Rotation` object from the specified quaternion. 109 110 The ordering of the quaternion is expected to be: `q0, q1, q2, q3` or 111 equivalently `qw, qx, qy, qz`. 112 113 Parameters 114 ---------- 115 quaternion : `Quaternion` 116 Desired quaternion from which to construct a Rotation object. 117 118 Returns 119 ------- 120 `Rotation` 121 The newly created Rotation instance. 122 """ 123 124 # Roll calculation 125 sinroll_cospitch = 2.0*(quaternion[0]*quaternion[1] + quaternion[2]*quaternion[3]) 126 cosroll_cospitch = 1.0 - 2.0*(quaternion[1]*quaternion[1] + quaternion[2]*quaternion[2]) 127 roll = sympy.atan2(sinroll_cospitch, cosroll_cospitch) 128 129 # Pitch calculation 130 sinpitch = 2.0*(quaternion[0]*quaternion[2] - quaternion[3]*quaternion[1]) 131 pitch = math.copysign(0.5 * math.pi, sinpitch) if not isinstance(sinpitch, Expr) \ 132 and abs(sinpitch) >= 1.0 else \ 133 sympy.asin(sinpitch) 134 135 # Yaw calculation 136 sinyaw_cospitch = 2.0*(quaternion[0]*quaternion[3] + quaternion[1]*quaternion[2]) 137 cosyaw_cospitch = 1.0 - 2.0*(quaternion[2]*quaternion[2] + quaternion[3]*quaternion[3]) 138 yaw = sympy.atan2(sinyaw_cospitch, cosyaw_cospitch) 139 return cls(identifier, roll=roll, pitch=pitch, yaw=yaw) 140 141 142 @classmethod 143 def from_rotation_matrix(cls, identifier: str, rotation_matrix: List[List[float]]) -> Rotation: 144 """Constructs a `Rotation` object from the specified rotation matrix. 145 146 Parameters 147 ---------- 148 rotation_matrix : `List[List[float]]` 149 Desired 3x3 rotation matrix from which to construct a Rotation object. 150 151 Returns 152 ------- 153 `Rotation` 154 The newly created Rotation instance. 155 """ 156 roll = sympy.atan2(rotation_matrix[2][1], rotation_matrix[2][2]) 157 pitch = -sympy.asin(rotation_matrix[2][0]) 158 yaw = sympy.atan2(rotation_matrix[1][0], rotation_matrix[0][0]) 159 return cls(identifier, roll=roll, pitch=pitch, yaw=yaw) 160 161 162 # Built-in method implementations -------------------------------------------------------------- 163 164 def __repr__(self) -> str: 165 roll = self.roll * 180.0 / math.pi 166 pitch = self.pitch * 180.0 / math.pi 167 yaw = self.yaw * 180.0 / math.pi 168 return 'roll = {}\u00b0, pitch = {}\u00b0, yaw = {}\u00b0'.format(roll, pitch, yaw) 169 170 def __eq__(self, other: Rotation) -> bool: 171 return (self.roll == other.roll) and (self.pitch == other.pitch) and (self.yaw == other.yaw) 172 173 def __copy__(self): 174 copy = self.__class__.__new__(self.__class__) 175 copy.__dict__.update(self.__dict__) 176 return copy 177 178 def __deepcopy__(self, memo): 179 copy = self.__class__.__new__(self.__class__) 180 memo[id(self)] = copy 181 for key, val in self.__dict__.items(): 182 setattr(copy, key, deepcopy(val, memo)) 183 return copy 184 185 186 # Public methods ------------------------------------------------------------------------------- 187 188 def clone(self) -> Rotation: 189 """Returns an exact clone of this `Rotation` instance.""" 190 return deepcopy(self) 191 192 193 def copy_from(self, other: Rotation) -> Rotation: 194 """Copies the rotation angles from another `Rotation` instance into this one. 195 196 The name of this instance will not be changed or overwritten. 197 198 Parameters 199 ---------- 200 other : `Rotation` 201 A Rotation object whose contents should be copied into this instance. 202 203 Returns 204 ------- 205 self : `Rotation` 206 The Rotation instance being manipulated. 207 """ 208 self.roll = other.roll 209 self.pitch = other.pitch 210 self.yaw = other.yaw 211 return self 212 213 214 def set(self, *, roll_deg: Union[float, Expr, None], 215 pitch_deg: Union[float, Expr, None], 216 yaw_deg: Union[float, Expr, None]) -> Rotation: 217 """Sets the roll, pitch, and yaw angles to the specified values. 218 219 Parameters 220 ---------- 221 roll_deg : `Union[float, sympy.Expr, None]` 222 Desired intrinsic roll angle in degrees. If `None` is specified, the angle will be 223 treated as a symbol. 224 pitch_deg : `Union[float, sympy.Expr, None]` 225 Desired intrinsic pitch angle in degrees. If `None` is specified, the angle will be 226 treated as a symbol. 227 yaw_deg : `Union[float, sympy.Expr, None]` 228 Desired intrinsic yaw angle in degrees. If `None` is specified, the angle will be 229 treated as a symbol. 230 231 Returns 232 ------- 233 self : `Rotation` 234 The Rotation instance being manipulated. 235 """ 236 self.roll = Symbol(self.name + '_roll') if roll_deg is None else \ 237 roll_deg * math.pi / 180.0 238 self.pitch = Symbol(self.name + '_pitch') if pitch_deg is None else \ 239 pitch_deg * math.pi / 180.0 240 self.yaw = Symbol(self.name + '_yaw') if yaw_deg is None else \ 241 yaw_deg * math.pi / 180.0 242 return self 243 244 245 def clear(self) -> Rotation: 246 """Clears all rotation angles to `0` degrees. 247 248 Returns 249 ------- 250 self : `Rotation` 251 The Rotation instance being manipulated. 252 """ 253 self.roll = self.pitch = self.yaw = 0.0 254 return self 255 256 257 def rotate_point(self, rotation_center: Tuple[float, float, float], 258 point: Tuple[float, float, float]) -> Tuple[float, float, float]: 259 """Rotates a `point` around its `rotation_center` according to the current `Rotation` 260 instance properties. 261 262 Parameters 263 ---------- 264 rotation_center : `Tuple[float, float, float]` 265 Cartestion coordinate around which to carry out the specified rotation. 266 point : `Tuple[float, float, float]` 267 The Cartesian coordinates of the point to be rotated. 268 269 Returns 270 ------- 271 `Tuple[float, float, float]` 272 The final Cartesian coordinates of the rotated point. 273 """ 274 R = self.get_rotation_matrix() 275 centered_point = [point[i] - rotation_center[i] for i in range(3)] 276 rotated_point = [sum(map(mul, R[i], centered_point)) for i in range(3)] 277 return tuple([rotation_center[i] + rotated_point[i] for i in range(3)]) 278 279 280 def get_quaternion(self) -> Quaternion: 281 """Returns a quaternion representing the `Rotation` object. 282 283 The ordering of the quaternion will be: `q0, q1, q2, q3` or 284 equivalently `qw, qx, qy, qz`. 285 286 Returns 287 ------- 288 `Quaternion` 289 A quaternion representing this Rotation object. 290 """ 291 half_roll = 0.5 * self.roll 292 half_pitch = 0.5 * self.pitch 293 half_yaw = 0.5 * self.yaw 294 s = sympy.cos(half_roll) * sympy.cos(half_pitch) * sympy.cos(half_yaw) \ 295 + sympy.sin(half_roll) * sympy.sin(half_pitch) * sympy.sin(half_yaw) 296 q1 = sympy.sin(half_roll) * sympy.cos(half_pitch) * sympy.cos(half_yaw) \ 297 - sympy.cos(half_roll) * sympy.sin(half_pitch) * sympy.sin(half_yaw) 298 q2 = sympy.cos(half_roll) * sympy.sin(half_pitch) * sympy.cos(half_yaw) \ 299 + sympy.sin(half_roll) * sympy.cos(half_pitch) * sympy.sin(half_yaw) 300 q3 = sympy.cos(half_roll) * sympy.cos(half_pitch) * sympy.sin(half_yaw) \ 301 - sympy.sin(half_roll) * sympy.sin(half_pitch) * sympy.cos(half_yaw) 302 return s, q1, q2, q3 303 304 305 def get_rotation_matrix_row(self, row_index: int) -> List[List[float]]: 306 """Returns a single row from the 3D rotation matrix representing the `Rotation` object. 307 308 Parameters 309 ---------- 310 row_index : `int` 311 Index of the rotation matrix row to return between [0, 2]. 312 313 Returns 314 ------- 315 `List[float]` 316 A 3-element list representing a single row from the rotation matrix for this Rotation object. 317 """ 318 if row_index == 0: 319 rotation_matrix0 = sympy.cos(self.pitch)*sympy.cos(self.yaw) 320 rotation_matrix1 = sympy.sin(self.roll)*sympy.sin(self.pitch)*sympy.cos(self.yaw) \ 321 - sympy.sin(self.yaw)*sympy.cos(self.roll) 322 rotation_matrix2 = sympy.sin(self.roll)*sympy.sin(self.yaw) \ 323 + sympy.sin(self.pitch)*sympy.cos(self.roll)*sympy.cos(self.yaw) 324 elif row_index == 1: 325 rotation_matrix0 = sympy.sin(self.yaw)*sympy.cos(self.pitch) 326 rotation_matrix1 = sympy.sin(self.roll)*sympy.sin(self.pitch)*sympy.sin(self.yaw) \ 327 + sympy.cos(self.roll)*sympy.cos(self.yaw) 328 rotation_matrix2 = sympy.sin(self.pitch)*sympy.sin(self.yaw)*sympy.cos(self.roll) \ 329 - sympy.sin(self.roll)*sympy.cos(self.yaw) 330 elif row_index == 2: 331 rotation_matrix0 = -sympy.sin(self.pitch) 332 rotation_matrix1 = sympy.sin(self.roll)*sympy.cos(self.pitch) 333 rotation_matrix2 = sympy.cos(self.roll)*sympy.cos(self.pitch) 334 else: 335 raise RuntimeError('Invalid row_index parameter ({})...must be between 0 and 2'.format(row_index)) 336 return [rotation_matrix0, rotation_matrix1, rotation_matrix2] 337 338 339 def get_rotation_matrix(self) -> List[List[float]]: 340 """Returns a 3D rotation matrix representing the `Rotation` object. 341 342 Returns 343 ------- 344 `List[List[float]]` 345 A 3x3 rotation matrix representing this Rotation object. 346 """ 347 rotation_matrix00 = sympy.cos(self.pitch)*sympy.cos(self.yaw) 348 rotation_matrix01 = sympy.sin(self.roll)*sympy.sin(self.pitch)*sympy.cos(self.yaw) \ 349 - sympy.sin(self.yaw)*sympy.cos(self.roll) 350 rotation_matrix02 = sympy.sin(self.roll)*sympy.sin(self.yaw) \ 351 + sympy.sin(self.pitch)*sympy.cos(self.roll)*sympy.cos(self.yaw) 352 rotation_matrix10 = sympy.sin(self.yaw)*sympy.cos(self.pitch) 353 rotation_matrix11 = sympy.sin(self.roll)*sympy.sin(self.pitch)*sympy.sin(self.yaw) \ 354 + sympy.cos(self.roll)*sympy.cos(self.yaw) 355 rotation_matrix12 = sympy.sin(self.pitch)*sympy.sin(self.yaw)*sympy.cos(self.roll) \ 356 - sympy.sin(self.roll)*sympy.cos(self.yaw) 357 rotation_matrix20 = -sympy.sin(self.pitch) 358 rotation_matrix21 = sympy.sin(self.roll)*sympy.cos(self.pitch) 359 rotation_matrix22 = sympy.cos(self.roll)*sympy.cos(self.pitch) 360 return [[rotation_matrix00, rotation_matrix01, rotation_matrix02], 361 [rotation_matrix10, rotation_matrix11, rotation_matrix12], 362 [rotation_matrix20, rotation_matrix21, rotation_matrix22]] 363 364 365 def as_tuple(self) -> Tuple[float, float, float]: 366 """Returns the current yaw-pitch-roll angles as a `tuple` in degrees.""" 367 return (self.yaw * 180.0 / math.pi, 368 self.pitch * 180.0 / math.pi, 369 self.roll * 180.0 / math.pi)
Represents a simple right-handed rotation assuming the nautical and aeronautical convention
of intrinsic yaw, pitch, roll rotation order.
Rotation(identifier: str, **kwargs)
49 def __init__(self, identifier: str, **kwargs) -> None: 50 """Initializes a `Rotation` instance, where `identifier` uniquely identifies the instance, 51 and `**kwargs` may contain the keywords `roll`, `pitch`, or `yaw` to specify a concrete 52 value for each component of the rotation. If any of these keywords are missing, the 53 corresponding angle of rotation will be assumed to be `0`. 54 55 Alternately, a `Rotation` instance can be created from: 56 57 - A quaternion, 58 - An explicit set of yaw, pitch, and roll angles, or 59 - A 3D rotation matrix. 60 61 A quaternion or rotation matrix is retrievable from any `Rotation` instance, regardless of 62 how it was created. 63 64 All rotations utilize a right-handed coordinate system and follow the convention of using 65 intrinsic rotations in the order `yaw, pitch, roll`. In other words, all rotations move in 66 the counter-clockwise direction when viewed from a positive axis looking toward origin, 67 first around the z-axis, then the y-axis, and finally around the x-axis. 68 """ 69 super().__init__() 70 self.name = identifier 71 self.roll = kwargs.get('roll', 0.0) 72 self.pitch = kwargs.get('pitch', 0.0) 73 self.yaw = kwargs.get('yaw', 0.0)
Initializes a Rotation instance, where identifier uniquely identifies the instance,
and **kwargs may contain the keywords roll, pitch, or yaw to specify a concrete
value for each component of the rotation. If any of these keywords are missing, the
corresponding angle of rotation will be assumed to be 0.
Alternately, a Rotation instance can be created from:
- A quaternion,
- An explicit set of yaw, pitch, and roll angles, or
- A 3D rotation matrix.
A quaternion or rotation matrix is retrievable from any Rotation instance, regardless of
how it was created.
All rotations utilize a right-handed coordinate system and follow the convention of using
intrinsic rotations in the order yaw, pitch, roll. In other words, all rotations move in
the counter-clockwise direction when viewed from a positive axis looking toward origin,
first around the z-axis, then the y-axis, and finally around the x-axis.
@classmethod
def
from_angles( cls, identifier: str, roll_rad: Union[float, sympy.core.expr.Expr, NoneType], pitch_rad: Union[float, sympy.core.expr.Expr, NoneType], yaw_rad: Union[float, sympy.core.expr.Expr, NoneType]) -> Rotation:
76 @classmethod 77 def from_angles(cls, identifier: str, 78 roll_rad: Union[float, Expr, None], 79 pitch_rad: Union[float, Expr, None], 80 yaw_rad: Union[float, Expr, None]) -> Rotation: 81 """Constructs a `Rotation` object with the specified roll, pitch, and yaw angles. 82 83 Parameters 84 ---------- 85 roll_rad : `Union[float, sympy.Expr, None]` 86 Desired intrinsic roll angle in radians. If `None` is specified, the angle will be 87 treated as a symbol. 88 pitch_rad : `Union[float, sympy.Expr, None]` 89 Desired intrinsic pitch angle in radians. If `None` is specified, the angle will be 90 treated as a symbol. 91 yaw_rad : `Union[float, sympy.Expr, None]` 92 Desired intrinsic yaw angle in radians. If `None` is specified, the angle will be 93 treated as a symbol. 94 95 Returns 96 ------- 97 `Rotation` 98 The newly created Rotation instance. 99 """ 100 roll = roll_rad if roll_rad is not None else Symbol(identifier + '_roll') 101 pitch = pitch_rad if pitch_rad is not None else Symbol(identifier + '_pitch') 102 yaw = yaw_rad if yaw_rad is not None else Symbol(identifier + '_yaw') 103 return cls(identifier, roll=roll, pitch=pitch, yaw=yaw)
Constructs a Rotation object with the specified roll, pitch, and yaw angles.
Parameters
- roll_rad (
Union[float, sympy.Expr, None]): Desired intrinsic roll angle in radians. IfNoneis specified, the angle will be treated as a symbol. - pitch_rad (
Union[float, sympy.Expr, None]): Desired intrinsic pitch angle in radians. IfNoneis specified, the angle will be treated as a symbol. - yaw_rad (
Union[float, sympy.Expr, None]): Desired intrinsic yaw angle in radians. IfNoneis specified, the angle will be treated as a symbol.
Returns
Rotation: The newly created Rotation instance.
@classmethod
def
from_quaternion( cls, identifier: str, quaternion: Tuple[Union[float, sympy.core.expr.Expr], Union[float, sympy.core.expr.Expr], Union[float, sympy.core.expr.Expr], Union[float, sympy.core.expr.Expr]]) -> Rotation:
106 @classmethod 107 def from_quaternion(cls, identifier: str, quaternion: Quaternion) -> Rotation: 108 """Constructs a `Rotation` object from the specified quaternion. 109 110 The ordering of the quaternion is expected to be: `q0, q1, q2, q3` or 111 equivalently `qw, qx, qy, qz`. 112 113 Parameters 114 ---------- 115 quaternion : `Quaternion` 116 Desired quaternion from which to construct a Rotation object. 117 118 Returns 119 ------- 120 `Rotation` 121 The newly created Rotation instance. 122 """ 123 124 # Roll calculation 125 sinroll_cospitch = 2.0*(quaternion[0]*quaternion[1] + quaternion[2]*quaternion[3]) 126 cosroll_cospitch = 1.0 - 2.0*(quaternion[1]*quaternion[1] + quaternion[2]*quaternion[2]) 127 roll = sympy.atan2(sinroll_cospitch, cosroll_cospitch) 128 129 # Pitch calculation 130 sinpitch = 2.0*(quaternion[0]*quaternion[2] - quaternion[3]*quaternion[1]) 131 pitch = math.copysign(0.5 * math.pi, sinpitch) if not isinstance(sinpitch, Expr) \ 132 and abs(sinpitch) >= 1.0 else \ 133 sympy.asin(sinpitch) 134 135 # Yaw calculation 136 sinyaw_cospitch = 2.0*(quaternion[0]*quaternion[3] + quaternion[1]*quaternion[2]) 137 cosyaw_cospitch = 1.0 - 2.0*(quaternion[2]*quaternion[2] + quaternion[3]*quaternion[3]) 138 yaw = sympy.atan2(sinyaw_cospitch, cosyaw_cospitch) 139 return cls(identifier, roll=roll, pitch=pitch, yaw=yaw)
Constructs a Rotation object from the specified quaternion.
The ordering of the quaternion is expected to be: q0, q1, q2, q3 or
equivalently qw, qx, qy, qz.
Parameters
- quaternion (
Quaternion): Desired quaternion from which to construct a Rotation object.
Returns
Rotation: The newly created Rotation instance.
@classmethod
def
from_rotation_matrix( cls, identifier: str, rotation_matrix: List[List[float]]) -> Rotation:
142 @classmethod 143 def from_rotation_matrix(cls, identifier: str, rotation_matrix: List[List[float]]) -> Rotation: 144 """Constructs a `Rotation` object from the specified rotation matrix. 145 146 Parameters 147 ---------- 148 rotation_matrix : `List[List[float]]` 149 Desired 3x3 rotation matrix from which to construct a Rotation object. 150 151 Returns 152 ------- 153 `Rotation` 154 The newly created Rotation instance. 155 """ 156 roll = sympy.atan2(rotation_matrix[2][1], rotation_matrix[2][2]) 157 pitch = -sympy.asin(rotation_matrix[2][0]) 158 yaw = sympy.atan2(rotation_matrix[1][0], rotation_matrix[0][0]) 159 return cls(identifier, roll=roll, pitch=pitch, yaw=yaw)
188 def clone(self) -> Rotation: 189 """Returns an exact clone of this `Rotation` instance.""" 190 return deepcopy(self)
Returns an exact clone of this Rotation instance.
193 def copy_from(self, other: Rotation) -> Rotation: 194 """Copies the rotation angles from another `Rotation` instance into this one. 195 196 The name of this instance will not be changed or overwritten. 197 198 Parameters 199 ---------- 200 other : `Rotation` 201 A Rotation object whose contents should be copied into this instance. 202 203 Returns 204 ------- 205 self : `Rotation` 206 The Rotation instance being manipulated. 207 """ 208 self.roll = other.roll 209 self.pitch = other.pitch 210 self.yaw = other.yaw 211 return self
def
set( self, *, roll_deg: Union[float, sympy.core.expr.Expr, NoneType], pitch_deg: Union[float, sympy.core.expr.Expr, NoneType], yaw_deg: Union[float, sympy.core.expr.Expr, NoneType]) -> Rotation:
214 def set(self, *, roll_deg: Union[float, Expr, None], 215 pitch_deg: Union[float, Expr, None], 216 yaw_deg: Union[float, Expr, None]) -> Rotation: 217 """Sets the roll, pitch, and yaw angles to the specified values. 218 219 Parameters 220 ---------- 221 roll_deg : `Union[float, sympy.Expr, None]` 222 Desired intrinsic roll angle in degrees. If `None` is specified, the angle will be 223 treated as a symbol. 224 pitch_deg : `Union[float, sympy.Expr, None]` 225 Desired intrinsic pitch angle in degrees. If `None` is specified, the angle will be 226 treated as a symbol. 227 yaw_deg : `Union[float, sympy.Expr, None]` 228 Desired intrinsic yaw angle in degrees. If `None` is specified, the angle will be 229 treated as a symbol. 230 231 Returns 232 ------- 233 self : `Rotation` 234 The Rotation instance being manipulated. 235 """ 236 self.roll = Symbol(self.name + '_roll') if roll_deg is None else \ 237 roll_deg * math.pi / 180.0 238 self.pitch = Symbol(self.name + '_pitch') if pitch_deg is None else \ 239 pitch_deg * math.pi / 180.0 240 self.yaw = Symbol(self.name + '_yaw') if yaw_deg is None else \ 241 yaw_deg * math.pi / 180.0 242 return self
Sets the roll, pitch, and yaw angles to the specified values.
Parameters
- roll_deg (
Union[float, sympy.Expr, None]): Desired intrinsic roll angle in degrees. IfNoneis specified, the angle will be treated as a symbol. - pitch_deg (
Union[float, sympy.Expr, None]): Desired intrinsic pitch angle in degrees. IfNoneis specified, the angle will be treated as a symbol. - yaw_deg (
Union[float, sympy.Expr, None]): Desired intrinsic yaw angle in degrees. IfNoneis specified, the angle will be treated as a symbol.
Returns
- self (
Rotation): The Rotation instance being manipulated.
245 def clear(self) -> Rotation: 246 """Clears all rotation angles to `0` degrees. 247 248 Returns 249 ------- 250 self : `Rotation` 251 The Rotation instance being manipulated. 252 """ 253 self.roll = self.pitch = self.yaw = 0.0 254 return self
Clears all rotation angles to 0 degrees.
Returns
- self (
Rotation): The Rotation instance being manipulated.
def
rotate_point( self, rotation_center: Tuple[float, float, float], point: Tuple[float, float, float]) -> Tuple[float, float, float]:
257 def rotate_point(self, rotation_center: Tuple[float, float, float], 258 point: Tuple[float, float, float]) -> Tuple[float, float, float]: 259 """Rotates a `point` around its `rotation_center` according to the current `Rotation` 260 instance properties. 261 262 Parameters 263 ---------- 264 rotation_center : `Tuple[float, float, float]` 265 Cartestion coordinate around which to carry out the specified rotation. 266 point : `Tuple[float, float, float]` 267 The Cartesian coordinates of the point to be rotated. 268 269 Returns 270 ------- 271 `Tuple[float, float, float]` 272 The final Cartesian coordinates of the rotated point. 273 """ 274 R = self.get_rotation_matrix() 275 centered_point = [point[i] - rotation_center[i] for i in range(3)] 276 rotated_point = [sum(map(mul, R[i], centered_point)) for i in range(3)] 277 return tuple([rotation_center[i] + rotated_point[i] for i in range(3)])
Rotates a point around its rotation_center according to the current Rotation
instance properties.
Parameters
- rotation_center (
Tuple[float, float, float]): Cartestion coordinate around which to carry out the specified rotation. - point (
Tuple[float, float, float]): The Cartesian coordinates of the point to be rotated.
Returns
Tuple[float, float, float]: The final Cartesian coordinates of the rotated point.
def
get_quaternion( self) -> Tuple[Union[float, sympy.core.expr.Expr], Union[float, sympy.core.expr.Expr], Union[float, sympy.core.expr.Expr], Union[float, sympy.core.expr.Expr]]:
280 def get_quaternion(self) -> Quaternion: 281 """Returns a quaternion representing the `Rotation` object. 282 283 The ordering of the quaternion will be: `q0, q1, q2, q3` or 284 equivalently `qw, qx, qy, qz`. 285 286 Returns 287 ------- 288 `Quaternion` 289 A quaternion representing this Rotation object. 290 """ 291 half_roll = 0.5 * self.roll 292 half_pitch = 0.5 * self.pitch 293 half_yaw = 0.5 * self.yaw 294 s = sympy.cos(half_roll) * sympy.cos(half_pitch) * sympy.cos(half_yaw) \ 295 + sympy.sin(half_roll) * sympy.sin(half_pitch) * sympy.sin(half_yaw) 296 q1 = sympy.sin(half_roll) * sympy.cos(half_pitch) * sympy.cos(half_yaw) \ 297 - sympy.cos(half_roll) * sympy.sin(half_pitch) * sympy.sin(half_yaw) 298 q2 = sympy.cos(half_roll) * sympy.sin(half_pitch) * sympy.cos(half_yaw) \ 299 + sympy.sin(half_roll) * sympy.cos(half_pitch) * sympy.sin(half_yaw) 300 q3 = sympy.cos(half_roll) * sympy.cos(half_pitch) * sympy.sin(half_yaw) \ 301 - sympy.sin(half_roll) * sympy.sin(half_pitch) * sympy.cos(half_yaw) 302 return s, q1, q2, q3
Returns a quaternion representing the Rotation object.
The ordering of the quaternion will be: q0, q1, q2, q3 or
equivalently qw, qx, qy, qz.
Returns
Quaternion: A quaternion representing this Rotation object.
def
get_rotation_matrix_row(self, row_index: int) -> List[List[float]]:
305 def get_rotation_matrix_row(self, row_index: int) -> List[List[float]]: 306 """Returns a single row from the 3D rotation matrix representing the `Rotation` object. 307 308 Parameters 309 ---------- 310 row_index : `int` 311 Index of the rotation matrix row to return between [0, 2]. 312 313 Returns 314 ------- 315 `List[float]` 316 A 3-element list representing a single row from the rotation matrix for this Rotation object. 317 """ 318 if row_index == 0: 319 rotation_matrix0 = sympy.cos(self.pitch)*sympy.cos(self.yaw) 320 rotation_matrix1 = sympy.sin(self.roll)*sympy.sin(self.pitch)*sympy.cos(self.yaw) \ 321 - sympy.sin(self.yaw)*sympy.cos(self.roll) 322 rotation_matrix2 = sympy.sin(self.roll)*sympy.sin(self.yaw) \ 323 + sympy.sin(self.pitch)*sympy.cos(self.roll)*sympy.cos(self.yaw) 324 elif row_index == 1: 325 rotation_matrix0 = sympy.sin(self.yaw)*sympy.cos(self.pitch) 326 rotation_matrix1 = sympy.sin(self.roll)*sympy.sin(self.pitch)*sympy.sin(self.yaw) \ 327 + sympy.cos(self.roll)*sympy.cos(self.yaw) 328 rotation_matrix2 = sympy.sin(self.pitch)*sympy.sin(self.yaw)*sympy.cos(self.roll) \ 329 - sympy.sin(self.roll)*sympy.cos(self.yaw) 330 elif row_index == 2: 331 rotation_matrix0 = -sympy.sin(self.pitch) 332 rotation_matrix1 = sympy.sin(self.roll)*sympy.cos(self.pitch) 333 rotation_matrix2 = sympy.cos(self.roll)*sympy.cos(self.pitch) 334 else: 335 raise RuntimeError('Invalid row_index parameter ({})...must be between 0 and 2'.format(row_index)) 336 return [rotation_matrix0, rotation_matrix1, rotation_matrix2]
Returns a single row from the 3D rotation matrix representing the Rotation object.
Parameters
- row_index (
int): Index of the rotation matrix row to return between [0, 2].
Returns
List[float]: A 3-element list representing a single row from the rotation matrix for this Rotation object.
def
get_rotation_matrix(self) -> List[List[float]]:
339 def get_rotation_matrix(self) -> List[List[float]]: 340 """Returns a 3D rotation matrix representing the `Rotation` object. 341 342 Returns 343 ------- 344 `List[List[float]]` 345 A 3x3 rotation matrix representing this Rotation object. 346 """ 347 rotation_matrix00 = sympy.cos(self.pitch)*sympy.cos(self.yaw) 348 rotation_matrix01 = sympy.sin(self.roll)*sympy.sin(self.pitch)*sympy.cos(self.yaw) \ 349 - sympy.sin(self.yaw)*sympy.cos(self.roll) 350 rotation_matrix02 = sympy.sin(self.roll)*sympy.sin(self.yaw) \ 351 + sympy.sin(self.pitch)*sympy.cos(self.roll)*sympy.cos(self.yaw) 352 rotation_matrix10 = sympy.sin(self.yaw)*sympy.cos(self.pitch) 353 rotation_matrix11 = sympy.sin(self.roll)*sympy.sin(self.pitch)*sympy.sin(self.yaw) \ 354 + sympy.cos(self.roll)*sympy.cos(self.yaw) 355 rotation_matrix12 = sympy.sin(self.pitch)*sympy.sin(self.yaw)*sympy.cos(self.roll) \ 356 - sympy.sin(self.roll)*sympy.cos(self.yaw) 357 rotation_matrix20 = -sympy.sin(self.pitch) 358 rotation_matrix21 = sympy.sin(self.roll)*sympy.cos(self.pitch) 359 rotation_matrix22 = sympy.cos(self.roll)*sympy.cos(self.pitch) 360 return [[rotation_matrix00, rotation_matrix01, rotation_matrix02], 361 [rotation_matrix10, rotation_matrix11, rotation_matrix12], 362 [rotation_matrix20, rotation_matrix21, rotation_matrix22]]
Returns a 3D rotation matrix representing the Rotation object.
Returns
List[List[float]]: A 3x3 rotation matrix representing this Rotation object.
def
as_tuple(self) -> Tuple[float, float, float]:
365 def as_tuple(self) -> Tuple[float, float, float]: 366 """Returns the current yaw-pitch-roll angles as a `tuple` in degrees.""" 367 return (self.yaw * 180.0 / math.pi, 368 self.pitch * 180.0 / math.pi, 369 self.roll * 180.0 / math.pi)
Returns the current yaw-pitch-roll angles as a tuple in degrees.