diff --git a/wpimath/algorithms.md b/wpimath/algorithms.md index 094c7c8ac5..ee15462431 100644 --- a/wpimath/algorithms.md +++ b/wpimath/algorithms.md @@ -598,69 +598,69 @@ Note that this reuses the cos(a\_y) cos(a\_z) and cos(a\_y) sin(a\_z) terms need ## Quaternion Exponential -We will take it as given that a quaternion has scalar and vector components `π‘ž = s + 𝑣⃗`, with vector component 𝑣⃗ consisting of a unit vector and magnitude `𝑣⃗ = ΞΈ * vΜ‚`. +We will take it as given that a quaternion has scalar and vector components `π‘ž = s + 𝑣⃗`, with vector component 𝑣⃗ consisting of a unit vector and magnitude `𝑣⃗ = ΞΈ vΜ‚`. ``` π‘ž = s + 𝑣⃗ -𝑣⃗ = ΞΈ * vΜ‚ +𝑣⃗ = ΞΈvΜ‚ exp(π‘ž) = exp(s + 𝑣⃗) -exp(π‘ž) = exp(s) * exp(𝑣⃗) -exp(π‘ž) = exp(s) * exp(ΞΈ * vΜ‚) +exp(π‘ž) = exp(s)exp(𝑣⃗) +exp(π‘ž) = exp(s)exp(ΞΈ vΜ‚) ``` Applying euler's identity: ``` -exp(ΞΈ * vΜ‚) = cos(ΞΈ) + sin(ΞΈ) * vΜ‚ +exp(ΞΈvΜ‚) = cos(ΞΈ) + sin(ΞΈ) vΜ‚ ``` Gives us: ``` -exp(π‘ž) = exp(s) * [cos(ΞΈ) + sin(ΞΈ) * vΜ‚] +exp(π‘ž) = exp(s) [cos(ΞΈ) + sin(ΞΈ) vΜ‚] ``` -Rearranging `𝑣⃗ = ΞΈ * vΜ‚` we can solve for vΜ‚: `vΜ‚ = 𝑣⃗ / ΞΈ` +Rearranging `𝑣⃗ = ΞΈ vΜ‚` we can solve for vΜ‚: `vΜ‚ = 𝑣⃗ / ΞΈ` ``` -exp(π‘ž) = exp(s) * [cos(ΞΈ) + sin(ΞΈ) / ΞΈ * 𝑣⃗] +exp(π‘ž) = exp(s) [cos(ΞΈ) + 𝑣⃗ sin(ΞΈ) / ΞΈ] ``` ## Quaternion Logarithm -We will take it as a given that for a given quaternion of the form `π‘ž = s + 𝑣⃗`, we can calculate the exponential: `exp(π‘ž) = exp(s) * [cos(ΞΈ) + sin(ΞΈ) / ΞΈ * 𝑣⃗]` where `ΞΈ = ||𝑣⃗||`. +We will take it as a given that for a given quaternion of the form `π‘ž = s + 𝑣⃗`, we can calculate the exponential: `exp(π‘ž) = exp(s) [cos(ΞΈ) + 𝑣⃗ sin(ΞΈ) / ΞΈ]` where `ΞΈ = ||𝑣⃗||`. Additionally, `exp(log(π‘ž)) = q` for a given value of `log(π‘ž)`. There are multiple solutions to `log(π‘ž)` caused by the imaginary axes in 𝑣⃗, discussed here: https://en.wikipedia.org/wiki/Complex_logarithm We will demonstrate the principal solution of `log(π‘ž)` satisfying `exp(log(π‘ž)) = q`. -This being `log(π‘ž) = log(||π‘ž||) + atan2(ΞΈ, s) / ΞΈ * 𝑣⃗`, is the principal solution to `log(π‘ž)` because the function `atan2(ΞΈ, s)` returns the principal value corresponding to its arguments. +This being `log(π‘ž) = log(||π‘ž||) + 𝑣⃗ atan2(ΞΈ, s) / ΞΈ`, is the principal solution to `log(π‘ž)` because the function `atan2(ΞΈ, s)` returns the principal value corresponding to its arguments. -Proof: `log(π‘ž) = log(||π‘ž||) + atan2(ΞΈ, s) / ΞΈ * 𝑣⃗` satisfies `exp(log(π‘ž)) = q`. +Proof: `log(π‘ž) = log(||π‘ž||) + 𝑣⃗ atan2(ΞΈ, s) / ΞΈ` satisfies `exp(log(π‘ž)) = q`. ``` -exp(log(π‘ž)) = exp(log(||π‘ž||) + atan2(ΞΈ, s) / ΞΈ * 𝑣⃗) +exp(log(π‘ž)) = exp(log(||π‘ž||) + 𝑣⃗ atan2(ΞΈ, s) / ΞΈ) -exp(log(π‘ž)) = exp(log(||π‘ž||)) * exp(atan2(ΞΈ, s) / ΞΈ * 𝑣⃗) +exp(log(π‘ž)) = exp(log(||π‘ž||)) exp(𝑣⃗ atan2(ΞΈ, s) / ΞΈ) Substitutions: -𝑣⃗ = ΞΈ * vΜ‚: +𝑣⃗ = ΞΈ vΜ‚: exp(log(||π‘ž||)) = ||π‘ž|| -exp(log(π‘ž)) = ||π‘ž|| * exp(atan2(ΞΈ, s) * vΜ‚) +exp(log(π‘ž)) = ||π‘ž|| exp(atan2(ΞΈ, s) vΜ‚) -exp(log(π‘ž)) = ||π‘ž|| * [cos(atan2(ΞΈ, s)) + sin(atan2(ΞΈ, s)) * vΜ‚] +exp(log(π‘ž)) = ||π‘ž|| [cos(atan2(ΞΈ, s)) + sin(atan2(ΞΈ, s)) vΜ‚] Substitutions: cos(atan2(ΞΈ, s)) = s / √(ΞΈΒ² + sΒ²) sin(atan2(ΞΈ, s)) = ΞΈ / √(ΞΈΒ² + sΒ²) -exp(log(π‘ž)) = ||π‘ž|| * [s / √(ΞΈΒ² + sΒ²) + ΞΈ / √(ΞΈΒ² + sΒ²) * vΜ‚] +exp(log(π‘ž)) = ||π‘ž|| [s / √(ΞΈΒ² + sΒ²) + ΞΈ vΜ‚ / √(ΞΈΒ² + sΒ²)] √(ΞΈΒ² + sΒ²) = ||π‘ž|| -exp(log(π‘ž)) = ||π‘ž|| * [s / ||π‘ž|| + ΞΈ / ||π‘ž|| * vΜ‚] -exp(log(π‘ž)) = s + ΞΈ * vΜ‚ +exp(log(π‘ž)) = ||π‘ž|| [s / ||π‘ž|| + ΞΈ vΜ‚ / ||π‘ž||] +exp(log(π‘ž)) = s + ΞΈ vΜ‚ exp(log(π‘ž)) = s + 𝑣⃗ @@ -669,37 +669,37 @@ exp(log(π‘ž)) = π‘ž ## Unit Quaternion in SO(3) from Rotation Vector in π–˜π–”(3) -We will take it as a given that members of π–˜π–”(3) take the form `𝑣⃗ = ΞΈ * vΜ‚`, representing a rotation ΞΈ around a unit axis vΜ‚. +We will take it as a given that members of π–˜π–”(3) take the form `𝑣⃗ = ΞΈ vΜ‚`, representing a rotation ΞΈ around a unit axis vΜ‚. -We additionally take it as a given that quaternions in SO(3) are of the form `π‘ž = cos(ΞΈ / 2) + sin(ΞΈ / 2) * vΜ‚`, representing a rotation of ΞΈ around unit axis vΜ‚. +We additionally take it as a given that quaternions in SO(3) are of the form `π‘ž = cos(ΞΈ / 2) + sin(ΞΈ / 2) vΜ‚`, representing a rotation of ΞΈ around unit axis vΜ‚. ``` ΞΈ = ||𝑣⃗|| vΜ‚ = 𝑣⃗ / ΞΈ -π‘ž = cos(ΞΈ / 2) + sin(ΞΈ / 2) * vΜ‚ -π‘ž = cos(||𝑣⃗|| / 2) + sin(||𝑣⃗|| / 2) / ||𝑣⃗|| * 𝑣⃗ +π‘ž = cos(ΞΈ / 2) + sin(ΞΈ / 2) vΜ‚ +π‘ž = cos(||𝑣⃗|| / 2) + sin(||𝑣⃗|| / 2) 𝑣⃗ / ||𝑣⃗|| ``` ## Rotation vector in π–˜π–”(3) from Unit Quaternion in SO(3) -We will take it as a given that members of π–˜π–”(3) take the form `π‘Ÿβƒ— = ΞΈ * rΜ‚`, representing a rotation ΞΈ around a unit axis rΜ‚. +We will take it as a given that members of π–˜π–”(3) take the form `π‘Ÿβƒ— = ΞΈ rΜ‚`, representing a rotation ΞΈ around a unit axis rΜ‚. -We additionally take it as a given that quaternions in SO(3) are of the form `π‘ž = s + 𝑣⃗ = cos(ΞΈ / 2) + sin(ΞΈ / 2) * vΜ‚`, representing a rotation of ΞΈ around unit axis vΜ‚. +We additionally take it as a given that quaternions in SO(3) are of the form `π‘ž = s + 𝑣⃗ = cos(ΞΈ / 2) + sin(ΞΈ / 2) vΜ‚`, representing a rotation of ΞΈ around unit axis vΜ‚. ``` -s + 𝑣⃗ = cos(ΞΈ / 2) + sin(ΞΈ / 2) * vΜ‚ +s + 𝑣⃗ = cos(ΞΈ / 2) + sin(ΞΈ / 2) vΜ‚ s = cos(ΞΈ / 2) -𝑣⃗ = sin(ΞΈ / 2) * vΜ‚ +𝑣⃗ = sin(ΞΈ / 2) vΜ‚ ||𝑣⃗|| = sin(ΞΈ / 2) ΞΈ / 2 = atan2(||𝑣⃗||, s) -ΞΈ = 2 * atan2(||𝑣⃗||, s) +ΞΈ = 2 atan2(||𝑣⃗||, s) rΜ‚ = 𝑣⃗ / ||𝑣⃗|| -π‘Ÿβƒ— = ΞΈ * rΜ‚ -π‘Ÿβƒ— = 2 * atan2(||𝑣⃗||, s) / ||𝑣⃗|| * 𝑣⃗ +π‘Ÿβƒ— = ΞΈrΜ‚ +π‘Ÿβƒ— = 2 atan2(||𝑣⃗||, s) 𝑣⃗ / ||𝑣⃗|| ``` ## Closed form solution for an Exponential Motion Profile