Source code for RigidBodyKinematics


# ISC License
#
# Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
#
# Permission to use, copy, modify, and/or distribute this software for any
# purpose with or without fee is hereby granted, provided that the above
# copyright notice and this permission notice appear in all copies.
#
# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.




import math

# Import required modules:
import numpy as np
from numpy import linalg as la

M_PI = np.pi
D2R = M_PI / 180.
R2D = 180. / M_PI


[docs]def Picheck(x): """ Picheck(x) Makes sure that the angle x lies within +/- Pi. """ if x > M_PI: return x - 2 * M_PI if x < -M_PI: return x + 2 * M_PI return x
[docs]def C2EP(C): """ C2EP Q = C2EP(C) translates the 3x3 direction cosine matrix C into the corresponding 4x1 euler parameter vector Q, where the first component of Q is the non-dimensional Euler parameter Beta_0 >= 0. Transformation is done using the Stanley method. """ tr = np.trace(C) b2 = np.array([(1 + tr) / 4, (1 + 2 * C[0, 0] - tr) / 4, (1 + 2 * C[1, 1] - tr) / 4, (1 + 2 * C[2, 2] - tr) / 4 ]) case = np.argmax(b2) b = b2 if case == 0: b[0] = np.sqrt(b2[0]) b[1] = (C[1, 2] - C[2, 1]) / 4 / b[0] b[2] = (C[2, 0] - C[0, 2]) / 4 / b[0] b[3] = (C[0, 1] - C[1, 0]) / 4 / b[0] elif case == 1: b[1] = np.sqrt(b2[1]) b[0] = (C[1, 2] - C[2, 1]) / 4 / b[1] if b[0] < 0: b[1] = -b[1] b[0] = -b[0] b[2] = (C[0, 1] + C[1, 0]) / 4 / b[1] b[3] = (C[2, 0] + C[0, 2]) / 4 / b[1] elif case == 2: b[2] = np.sqrt(b2[2]) b[0] = (C[2, 0] - C[0, 2]) / 4 / b[2] if b[0] < 0: b[2] = -b[2] b[0] = -b[0] b[1] = (C[0, 1] + C[1, 0]) / 4 / b[2] b[3] = (C[1, 2] + C[2, 1]) / 4 / b[2] elif case == 3: b[3] = np.sqrt(b2[3]) b[0] = (C[0, 1] - C[1, 0]) / 4 / b[3] if b[0] < 0: b[3] = -b[3] b[0] = -b[0] b[1] = (C[2, 0] + C[0, 2]) / 4 / b[3] b[2] = (C[1, 2] + C[2, 1]) / 4 / b[3] return b
[docs]def C2Euler121(C): """ C2Euler121 Q = C2Euler121(C) translates the 3x3 direction cosine matrix C into the corresponding (1-2-1) euler angle set. """ q0 = math.atan2(C[0, 1], -C[0, 2]) q1 = math.acos(C[0, 0]) q2 = math.atan2(C[1, 0], C[2, 0]) q = np.array([q0, q1, q2]) return q
[docs]def C2Euler123(C): """ C2Euler123 Q = C2Euler123(C) translates the 3x3 direction cosine matrix C into the corresponding (1-2-3) euler angle set. """ q0 = np.arctan2(-C[2, 1], C[2, 2]) q1 = np.arcsin(C[2, 0]) q2 = np.arctan2(-C[1, 0], C[0, 0]) q = np.array([q0, q1, q2]) return q
[docs]def C2Euler131(C): """ C2Euler131 Q = C2Euler131(C) translates the 3x3 direction cosine matrix C into the corresponding (1-3-1) euler angle set. """ q0 = math.atan2(C[0, 2], C[0, 1]) q1 = math.acos(C[0, 0]) q2 = math.atan2(C[2, 0], -C[1, 0]) q = np.array([q0, q1, q2]) return q
[docs]def C2Euler132(C): """ C2Euler132 Q = C2Euler132(C) translates the 3x3 direction cosine matrix C into the corresponding (1-3-2) euler angle set. """ q0 = math.atan2(C[1, 2], C[1, 1]) q1 = math.asin(-C[1, 0]) q2 = math.atan2(C[2, 0], C[0, 0]) q = np.array([q0, q1, q2]) return q
[docs]def C2Euler212(C): """ C2Euler212 Q = C2Euler212(C) translates the 3x3 direction cosine matrix C into the corresponding (2-1-2) euler angle set. """ q0 = math.atan2(C[1, 0], C[1, 2]) q1 = math.acos(C[1, 1]) q2 = math.atan2(C[0, 1], -C[2, 1]) q = np.array([q0, q1, q2]) return q
[docs]def C2Euler213(C): """ C2Euler213 Q = C2Euler213(C) translates the 3x3 direction cosine matrix C into the corresponding (2-1-3) euler angle set. """ q0 = math.atan2(C[2, 0], C[2, 2]) q1 = math.asin(-C[2, 1]) q2 = math.atan2(C[0, 1], C[1, 1]) q = np.array([q0, q1, q2]) return q
[docs]def C2Euler231(C): """ C2Euler231 Q = C2Euler231(C) translates the 3x3 direction cosine matrix C into the corresponding (2-3-1) euler angle set. """ q0 = math.atan2(-C[0, 2], C[0, 0]) q1 = math.asin(C[0, 1]) q2 = math.atan2(-C[2, 1], C[1, 1]) q = np.array([q0, q1, q2]) return q
[docs]def C2Euler232(C): """ C2Euler232 Q = C2Euler232(C) translates the 3x3 direction cosine matrix C into the corresponding (2-3-2) euler angle set. """ q0 = math.atan2(C[1, 2], -C[1, 0]) q1 = math.acos(C[1, 1]) q2 = math.atan2(C[2, 1], C[0, 1]) q = np.array([q0, q1, q2]) return q
[docs]def C2Euler312(C): """ C2Euler312 Q = C2Euler312(C) translates the 3x3 direction cosine matrix C into the corresponding (3-1-2) euler angle set. """ q0 = math.atan2(-C[1, 0], C[1, 1]) q1 = math.asin(C[1, 2]) q2 = math.atan2(-C[0, 2], C[2, 2]) q = np.array([q0, q1, q2]) return q
[docs]def C2Euler313(C): """ C2Euler313 Q = C2Euler313(C) translates the 3x3 direction cosine matrix C into the corresponding (3-1-3) euler angle set. """ q0 = math.atan2(C[2, 0], -C[2, 1]) q1 = math.acos(C[2, 2]) q2 = math.atan2(C[0, 2], C[1, 2]) q = np.array([q0, q1, q2]) return q
[docs]def C2Euler321(C): """ C2Euler321 Q = C2Euler321(C) translates the 3x3 direction cosine matrix C into the corresponding (3-2-1) euler angle set. """ q0 = math.atan2(C[0, 1], C[0, 0]) q1 = math.asin(-C[0, 2]) q2 = math.atan2(C[1, 2], C[2, 2]) q = np.array([q0, q1, q2]) return q
[docs]def C2Euler323(C): """ C2Euler323 Q = C2Euler323(C) translates the 3x3 direction cosine matrix C into the corresponding (3-2-3) euler angle set. """ q0 = math.atan2(C[2, 1], C[2, 0]) q1 = math.acos(C[2, 2]) q2 = math.atan2(C[1, 2], -C[0, 2]) q = np.array([q0, q1, q2]) return q
[docs]def C2Gibbs(C): """ C2Gibbs Q = C2Gibbs(C) translates the 3x3 direction cosine matrix C into the corresponding 3x1 gibbs vector Q. """ b = C2EP(C) q0 = b[1] / b[0] q1 = b[2] / b[0] q2 = b[3] / b[0] q = np.array([q0, q1, q2]) return q
[docs]def C2MRP(C): """ C2MRP Q = C2MRP(C) translates the 3x3 direction cosine matrix C into the corresponding 3x1 MRP vector Q where the MRP vector is chosen such that :math:`|Q| <= 1`. """ b = C2EP(C) q = np.array([ b[1] / (1 + b[0]), b[2] / (1 + b[0]), b[3] / (1 + b[0]) ]) return q
[docs]def C2PRV(C): """ C2PRV Q = C2PRV(C) translates the 3x3 direction cosine matrix C into the corresponding 3x1 principal rotation vector Q, where the first component of Q is the principal rotation angle phi (0<= phi <= Pi) """ cp = (np.trace(C) - 1) / 2 p = np.arccos(cp) sp = p / 2. / np.sin(p) q = np.array([ (C[1, 2] - C[2, 1]) * sp, (C[2, 0] - C[0, 2]) * sp, (C[0, 1] - C[1, 0]) * sp ]) return q
[docs]def addEP(b1, b2): """ addEP(B1,B2) Q = addEP(B1,B2) provides the euler parameter vector which corresponds to performing to successive rotations B1 and B2. """ q0 = b2[0] * b1[0] - b2[1] * b1[1] - b2[2] * b1[2] - b2[3] * b1[3] q1 = b2[1] * b1[0] + b2[0] * b1[1] + b2[3] * b1[2] - b2[2] * b1[3] q2 = b2[2] * b1[0] - b2[3] * b1[1] + b2[0] * b1[2] + b2[1] * b1[3] q3 = b2[3] * b1[0] + b2[2] * b1[1] - b2[1] * b1[2] + b2[0] * b1[3] q = np.array([q0, q1, q2, q3]) return q
[docs]def addEuler121(e1, e2): """ addEuler121(E1,E2) Q = addEuler121(E1,E2) computes the overall (1-2-1) euler angle vector corresponding to two successive (1-2-1) rotations E1 and E2. """ cp1 = math.cos(e1[1]) cp2 = math.cos(e2[1]) sp1 = math.sin(e1[1]) sp2 = math.sin(e2[1]) dum = e1[2] + e2[0] q1 = math.acos(cp1 * cp2 - sp1 * sp2 * math.cos(dum)) cp3 = math.cos(q1) q0 = Picheck(e1[0] + math.atan2(sp1 * sp2 * math.sin(dum), cp2 - cp3 * cp1)) q2 = Picheck(e2[2] + math.atan2(sp1 * sp2 * math.sin(dum), cp1 - cp3 * cp2)) q = np.array([q0, q1, q2]) return q
[docs]def addEuler123(e1, e2): """ addEuler123(E1,E2) Q = addEuler123(E1,E2) computes the overall (1-2-3) euler angle vector corresponding to two successive (1-2-3) rotations E1 and E2. """ C1 = euler1232C(e1) C2 = euler1232C(e2) C = np.dot(C2, C1) return C2Euler123(C)
[docs]def addEuler131(e1, e2): """ addEuler131(E1,E2) Q = addEuler131(E1,E2) computes the overall (1-3-1) euler angle vector corresponding to two successive (1-3-1) rotations E1 and E2. """ cp1 = math.cos(e1[1]) cp2 = math.cos(e2[1]) sp1 = math.sin(e1[1]) sp2 = math.sin(e2[1]) dum = e1[2] + e2[0] q1 = math.acos(cp1 * cp2 - sp1 * sp2 * math.cos(dum)) cp3 = math.cos(q1) q0 = Picheck(e1[0] + math.atan2(sp1 * sp2 * math.sin(dum), cp2 - cp3 * cp1)) q2 = Picheck(e2[2] + math.atan2(sp1 * sp2 * math.sin(dum), cp1 - cp3 * cp2)) q = np.array([q0, q1, q2]) return q
[docs]def addEuler132(e1, e2): """ addEuler132(E1,E2) Q = addEuler132(E1,E2) computes the overall (1-3-2) euler angle vector corresponding to two successive (1-3-2) rotations E1 and E2. """ C1 = euler1322C(e1) C2 = euler1322C(e2) C = np.dot(C2, C1) return C2Euler132(C)
[docs]def addEuler212(e1, e2): """ addEuler212(E1,E2) Q = addEuler212(E1,E2) computes the overall (2-1-2) euler angle vector corresponding to two successive (2-1-2) rotations E1 and E2. """ cp1 = math.cos(e1[1]) cp2 = math.cos(e2[1]) sp1 = math.sin(e1[1]) sp2 = math.sin(e2[1]) dum = e1[2] + e2[0] q1 = math.acos(cp1 * cp2 - sp1 * sp2 * math.cos(dum)) cp3 = math.cos(q1) q0 = Picheck(e1[0] + math.atan2(sp1 * sp2 * math.sin(dum), cp2 - cp3 * cp1)) q2 = Picheck(e2[2] + math.atan2(sp1 * sp2 * math.sin(dum), cp1 - cp3 * cp2)) q = np.array([q0, q1, q2]) return q
[docs]def addEuler213(e1, e2): """ addEuler213(E1,E2) Q = addEuler213(E1,E2) computes the overall (2-1-3) euler angle vector corresponding to two successive (2-1-3) rotations E1 and E2. """ C1 = euler2132C(e1) C2 = euler2132C(e2) C = np.dot(C2, C1) return C2Euler213(C)
[docs]def addEuler231(e1, e2): """ addEuler231(E1,E2) Q = addEuler231(E1,E2) computes the overall (2-3-1) euler angle vector corresponding to two successive (2-3-1) rotations E1 and E2. """ C1 = euler2312C(e1) C2 = euler2312C(e2) C = np.dot(C2, C1) return C2Euler231(C)
[docs]def addEuler232(e1, e2): """ addEuler232(E1,E2) Q = addEuler232(E1,E2) computes the overall (2-3-2) euler angle vector corresponding to two successive (2-3-2) rotations E1 and E2. """ cp1 = math.cos(e1[1]) cp2 = math.cos(e2[1]) sp1 = math.sin(e1[1]) sp2 = math.sin(e2[1]) dum = e1[2] + e2[0] q1 = math.acos(cp1 * cp2 - sp1 * sp2 * math.cos(dum)) cp3 = math.cos(q1) q0 = Picheck(e1[0] + math.atan2(sp1 * sp2 * math.sin(dum), cp2 - cp3 * cp1)) q2 = Picheck(e2[2] + math.atan2(sp1 * sp2 * math.sin(dum), cp1 - cp3 * cp2)) q = np.array([q0, q1, q2]) return q
[docs]def addEuler312(e1, e2): """ addEuler312(E1,E2) Q = addEuler312(E1,E2) computes the overall (3-1-2) euler angle vector corresponding to two successive (3-1-2) rotations E1 and E2. """ C1 = euler3122C(e1) C2 = euler3122C(e2) C = np.dot(C2, C1) return C2Euler312(C)
[docs]def addEuler313(e1, e2): """ addEuler313(E1,E2) Q = addEuler313(E1,E2) computes the overall (3-1-3) euler angle vector corresponding to two successive (3-1-3) rotations E1 and E2. """ cp1 = math.cos(e1[1]) cp2 = math.cos(e2[1]) sp1 = math.sin(e1[1]) sp2 = math.sin(e2[1]) dum = e1[2] + e2[0] q1 = math.acos(cp1 * cp2 - sp1 * sp2 * math.cos(dum)) cp3 = math.cos(q1) q0 = Picheck(e1[0] + math.atan2(sp1 * sp2 * math.sin(dum), cp2 - cp3 * cp1)) q2 = Picheck(e2[2] + math.atan2(sp1 * sp2 * math.sin(dum), cp1 - cp3 * cp2)) q = np.array([q0, q1, q2]) return q
[docs]def addEuler321(e1, e2): """ addEuler321(E1,E2) Q = addEuler321(E1,E2) computes the overall (3-2-1) euler angle vector corresponding to two successive (3-2-1) rotations E1 and E2. """ C1 = euler3212C(e1) C2 = euler3212C(e2) C = np.dot(C2, C1) return C2Euler321(C)
[docs]def addEuler323(e1, e2): """ addEuler323(E1,E2) Q = addEuler323(E1,E2) computes the overall (3-2-3) euler angle vector corresponding to two successive (3-2-3) rotations E1 and E2. """ cp1 = math.cos(e1[1]) cp2 = math.cos(e2[1]) sp1 = math.sin(e1[1]) sp2 = math.sin(e2[1]) dum = e1[2] + e2[0] q1 = math.acos(cp1 * cp2 - sp1 * sp2 * math.cos(dum)) cp3 = math.cos(q1) q0 = Picheck(e1[0] + math.atan2(sp1 * sp2 * math.sin(dum), cp2 - cp3 * cp1)) q2 = Picheck(e2[2] + math.atan2(sp1 * sp2 * math.sin(dum), cp1 - cp3 * cp2)) q = np.array([q0, q1, q2]) return q
[docs]def addGibbs(q1, q2): """ addGibbs(Q1,Q2) Q = addGibbs(Q1,Q2) provides the gibbs vector which corresponds to performing to successive rotations Q1 and Q2. """ result = (q1 + q2 + np.cross(q1, q2)) / (1 - np.dot(q1, q2)) return result
[docs]def addMRP(q1, q2): """ addMRP(Q1,Q2) Q = addMRP(Q1,Q2) provides the MRP vector which corresponds to performing to successive rotations Q1 and Q2. """ den = 1 + np.dot(q1, q1) * np.dot(q2, q2) - 2 * np.dot(q1, q2) if np.abs(den) < 1e-5: q2 = -q2/np.dot(q2,q2) den = 1 + np.dot(q1, q1) * np.dot(q2, q2) - 2 * np.dot(q1, q2) num = (1 - np.dot(q1, q1)) * q2 + (1 - np.dot(q2, q2)) * q1 + 2 * np.cross(q1, q2) q = num / den if np.dot(q,q) > 1: q = - q/np.dot(q,q) return q
[docs]def PRV2elem(r): """ PRV2elem(R) Q = PRV2elem(R) translates a prinicpal rotation vector R into the corresponding principal rotation element set Q. """ q0 = np.linalg.norm(r) if q0 < 1e-12: return np.zeros(4) q1 = r[0] / q0 q2 = r[1] / q0 q3 = r[2] / q0 q = np.array([q0, q1, q2, q3]) return q
[docs]def addPRV(qq1, qq2): """ addPRV(Q1,Q2) Q = addPRV(Q1,Q2) provides the principal rotation vector which corresponds to performing to successive prinicipal rotations Q1 and Q2. """ q1 = PRV2elem(qq1) q2 = PRV2elem(qq2) cp1 = math.cos(q1[0] / 2.) cp2 = math.cos(q2[0] / 2.) sp1 = math.sin(q1[0] / 2.) sp2 = math.sin(q2[0] / 2.) e1 = q1[1:4] e2 = q2[1:4] p = 2. * math.acos(cp1 * cp2 - sp1 * sp2 * np.dot(e1, e2)) sp = math.sin(p / 2.) e = (cp1 * sp2 * e2 + cp2 * sp1 * e1 + sp1 * sp2 * np.cross(e1, e2)) q = (p / sp) * e return q
[docs]def BinvEP(q): """ BinvEP(Q) B = BinvEP(Q) returns the 3x4 matrix which relates the derivative of euler parameter vector Q to the body angular velocity vector w. w = 2 [B(Q)]^(-1) dQ/dt """ B = np.zeros([3, 4]) B[0, 0] = -q[1] B[0, 1] = q[0] B[0, 2] = q[3] B[0, 3] = -q[2] B[1] = -q[2] B[1, 1] = -q[3] B[1, 2] = q[0] B[1, 3] = q[1] B[2] = -q[3] B[2, 1] = q[2] B[2, 2] = -q[1] B[2, 3] = q[0] return B
[docs]def BinvEuler121(q): """ BinvEuler121(Q) B = BinvEuler121(Q) returns the 3x3 matrix which relates the derivative of the (1-2-1) euler angle vector Q to the body angular velocity vector w. w = [B(Q)]^(-1) dQ/dt """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = c2 B[0, 1] = 0 B[0, 2] = 1 B[1, 0] = s2 * s3 B[1, 1] = c3 B[1, 2] = 0 B[2, 0] = s2 * c3 B[2, 1] = -s3 B[2, 2] = 0 return B
[docs]def BinvEuler123(q): """ BinvEuler123(Q) B = BinvEuler123(Q) returns the 3x3 matrix which relates the derivative of the (1-2-3) euler angle vector Q to the body angular velocity vector w. w = [B(Q)]^(-1) dQ/dt """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = c2 * c3 B[0, 1] = s3 B[0, 2] = 0 B[1, 0] = -c2 * s3 B[1, 1] = c3 B[1, 2] = 0 B[2, 0] = s2 B[2, 1] = 0 B[2, 2] = 1 return B
[docs]def BinvEuler131(q): """ BinvEuler131(Q) B = BinvEuler131(Q) returns the 3x3 matrix which relates the derivative of the (1-3-1) euler angle vector Q to the body angular velocity vector w. w = [B(Q)]^(-1) dQ/dt """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = c2 B[0, 1] = 0 B[0, 2] = 1 B[1, 0] = -s2 * c3 B[1, 1] = s3 B[1, 2] = 0 B[2, 0] = s2 * s3 B[2, 1] = c3 B[2, 2] = 0 return B
[docs]def BinvEuler132(q): """ BinvEuler132(Q) B = BinvEuler132(Q) returns the 3x3 matrix which relates the derivative of the (1-3-2) euler angle vector Q to the body angular velocity vector w. w = [B(Q)]^(-1) dQ/dt """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = c2 * c3 B[0, 1] = -s3 B[0, 2] = 0 B[1, 0] = -s2 B[1, 1] = 0 B[1, 2] = 1 B[2, 0] = c2 * s3 B[2, 1] = c3 B[2, 2] = 0 return B
[docs]def BinvEuler212(q): """ BinvEuler212(Q) B = BinvEuler212(Q) returns the 3x3 matrix which relates the derivative of the (2-1-2) euler angle vector Q to the body angular velocity vector w. w = [B(Q)]^(-1) dQ/dt """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = s2 * s3 B[0, 1] = c3 B[0, 2] = 0 B[1, 0] = c2 B[1, 1] = 0 B[1, 2] = 1 B[2, 0] = -s2 * c3 B[2, 1] = s3 B[2, 2] = 0 return B
[docs]def BinvEuler213(q): """ BinvEuler213(Q) B = BinvEuler213(Q) returns the 3x3 matrix which relates the derivative of the (2-1-3) euler angle vector Q to the body angular velocity vector w. w = [B(Q)]^(-1) dQ/dt """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = c2 * s3 B[0, 1] = c3 B[0, 2] = 0 B[1, 0] = c2 * c3 B[1, 1] = -s3 B[1, 2] = 0 B[2, 0] = -s2 B[2, 1] = 0 B[2, 2] = 1 return B
[docs]def BinvEuler231(q): """ BinvEuler231(Q) B = BinvEuler231(Q) returns the 3x3 matrix which relates the derivative of the (2-3-1) euler angle vector Q to the body angular velocity vector w. w = [B(Q)]^(-1) dQ/dt """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = s2 B[0, 1] = 0 B[0, 2] = 1 B[1, 0] = c2 * c3 B[1, 1] = s3 B[1, 2] = 0 B[2, 0] = -c2 * s3 B[2, 1] = c3 B[2, 2] = 0 return B
[docs]def BinvEuler232(q): """ BinvEuler232(Q) B = BinvEuler232(Q) returns the 3x3 matrix which relates the derivative of the (2-3-2) euler angle vector Q to the body angular velocity vector w. w = [B(Q)]^(-1) dQ/dt """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = s2 * c3 B[0, 1] = -s3 B[0, 2] = 0 B[1, 0] = c2 B[1, 1] = 0 B[1, 2] = 1 B[2, 0] = s2 * s3 B[2, 1] = c3 B[2, 2] = 0 return B
[docs]def BinvEuler312(q): """ BinvEuler312(Q) B = BinvEuler312(Q) returns the 3x3 matrix which relates the derivative of the (3-1-2) euler angle vector Q to the body angular velocity vector w. w = [B(Q)]^(-1) dQ/dt """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = -c2 * s3 B[0, 1] = c3 B[0, 2] = 0 B[1, 0] = s2 B[1, 1] = 0 B[1, 2] = 1 B[2, 0] = c2 * c3 B[2, 1] = s3 B[2, 2] = 0 return B
[docs]def BinvEuler313(q): """ BinvEuler313(Q) B = BinvEuler313(Q) returns the 3x3 matrix which relates the derivative of the (3-1-3) euler angle vector Q to the body angular velocity vector w. w = [B(Q)]^(-1) dQ/dt """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = s2 * s3 B[0, 1] = c3 B[0, 2] = 0 B[1, 0] = s2 * c3 B[1, 1] = -s3 B[1, 2] = 0 B[2, 0] = c2 B[2, 1] = 0 B[2, 2] = 1 return B
[docs]def BinvEuler321(q): """ BinvEuler321(Q) B = BinvEuler321(Q) returns the 3x3 matrix which relates the derivative of the (3-2-1) euler angle vector Q to the body angular velocity vector w. w = [B(Q)]^(-1) dQ/dt """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = -s2 B[0, 1] = 0 B[0, 2] = 1 B[1, 0] = c2 * s3 B[1, 1] = c3 B[1, 2] = 0 B[2, 0] = c2 * c3 B[2, 1] = -s3 B[2, 2] = 0 return B
[docs]def BinvEuler323(q): """ BinvEuler323(Q) B = BinvEuler323(Q) returns the 3x3 matrix which relates the derivative of the (3-2-3) euler angle vector Q to the body angular velocity vector w. w = [B(Q)]^(-1) dQ/dt """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = -s2 * c3 B[0, 1] = s3 B[0, 2] = 0 B[1, 0] = s2 * s3 B[1, 1] = c3 B[1, 2] = 0 B[2, 0] = c2 B[2, 1] = 0 B[2, 2] = 1 return B
[docs]def BinvGibbs(q): """ BinvGibbs(Q) B = BinvGibbs(Q) returns the 3x3 matrix which relates the derivative of gibbs vector Q to the body angular velocity vector w. w = 2 [B(Q)]^(-1) dQ/dt """ B = np.zeros([3, 3]) B[0, 0] = 1 B[0, 1] = q[2] B[0, 2] = -q[1] B[1, 0] = -q[2] B[1, 1] = 1 B[1, 2] = q[0] B[2, 0] = q[1] B[2, 1] = -q[0] B[2, 2] = 1 B = B / (1 + np.dot(q, q)) return B
[docs]def BinvMRP(q): """ BinvMRP(Q) B = BinvMRP(Q) returns the 3x3 matrix which relates the derivative of MRP vector Q to the body angular velocity vector w. w = 4 [B(Q)]^(-1) dQ/dt """ s2 = np.dot(q, q) B = np.zeros([3, 3]) B[0, 0] = 1 - s2 + 2 * q[0] * q[0] B[0, 1] = 2 * (q[0] * q[1] + q[2]) B[0, 2] = 2 * (q[0] * q[2] - q[1]) B[1, 0] = 2 * (q[1] * q[0] - q[2]) B[1, 1] = 1 - s2 + 2 * q[1] * q[1] B[1, 2] = 2 * (q[1] * q[2] + q[0]) B[2, 0] = 2 * (q[2] * q[0] + q[1]) B[2, 1] = 2 * (q[2] * q[1] - q[0]) B[2, 2] = 1 - s2 + 2 * q[2] * q[2] B = B / (1 + s2) / (1 + s2) return B
[docs]def BinvPRV(q): """ BinvPRV(Q) B = BinvPRV(Q) returns the 3x3 matrix which relates the derivative of principal rotation vector Q to the body angular velocity vector w. w = [B(Q)]^(-1) dQ/dt """ p = la.norm(q) c1 = (1 - math.cos(p)) / p / p c2 = (p - math.sin(p)) / p / p / p B = np.zeros([3, 3]) B[0, 0] = 1 - c2 * (q[1] * q[1] + q[2] * q[2]) B[0, 1] = c1 * q[2] + c2 * q[0] * q[1] B[0, 2] = -c1 * q[1] + c2 * q[0] * q[2] B[1, 0] = -c1 * q[2] + c2 * q[0] * q[1] B[1, 1] = 1 - c2 * (q[0] * q[0] + q[2] * q[2]) B[1, 2] = c1 * q[0] + c2 * q[1] * q[2] B[2, 0] = c1 * q[1] + c2 * q[2] * q[0] B[2, 1] = -c1 * q[0] + c2 * q[2] * q[1] B[2, 2] = 1 - c2 * (q[0] * q[0] + q[1] * q[1]) return B
[docs]def BmatEP(q): """ BmatEP(Q) B = BmatEP(Q) returns the 4x3 matrix which relates the body angular velocity vector w to the derivative of Euler parameter vector Q. dQ/dt = 1/2 [B(Q)] w """ B = np.zeros([4, 3]) B[0, 0] = -q[1] B[0, 1] = -q[2] B[0, 2] = -q[3] B[1, 0] = q[0] B[1, 1] = -q[3] B[1, 2] = q[2] B[2, 0] = q[3] B[2, 1] = q[0] B[2, 2] = -q[1] B[3, 0] = -q[2] B[3, 1] = q[1] B[3, 2] = q[0] return B
[docs]def BmatEuler121(q): """ BmatEuler121(Q) B = BmatEuler121(Q) returns the 3x3 matrix which relates the body angular velocity vector w to the derivative of (1-2-1) euler angle vector Q. dQ/dt = [B(Q)] w """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = 0 B[0, 1] = s3 B[0, 2] = c3 B[1, 0] = 0 B[1, 1] = s2 * c3 B[1, 2] = -s2 * s3 B[2, 0] = s2 B[2, 1] = -c2 * s3 B[2, 2] = -c2 * c3 B = B / s2 return B
[docs]def BmatEuler123(q): """ BmatEuler123(Q) B = BmatEuler123(Q) returns the 3x3 matrix which relates the body angular velocity vector w to the derivative of (1-2-3) euler angle vector Q. dQ/dt = [B(Q)] w """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = c3 B[0, 1] = -s3 B[0, 2] = 0 B[1, 0] = c2 * s3 B[1, 1] = c2 * c3 B[1, 2] = 0 B[2, 0] = -s2 * c3 B[2, 1] = s2 * s3 B[2, 2] = c2 B = B / c2 return B
[docs]def BmatEuler131(q): """ BmatEuler131(Q) B = BmatEuler131(Q) returns the 3x3 matrix which relates the body angular velocity vector w to the derivative of (1-3-1) euler angle vector Q. dQ/dt = [B(Q)] w """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = 0 B[0, 1] = -c3 B[0, 2] = s3 B[1, 0] = 0 B[1, 1] = s2 * s3 B[1, 2] = s2 * c3 B[2, 0] = s2 B[2, 1] = c2 * c3 B[2, 2] = -c2 * s3 B = B / s2 return B
[docs]def BmatEuler132(q): """ BmatEuler132(Q) B = BmatEuler132(Q) returns the 3x3 matrix which relates the body angular velocity vector w to the derivative of (1-3-2) euler angle vector Q. dQ/dt = [B(Q)] w """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = c3 B[0, 1] = 0 B[0, 2] = s3 B[1, 0] = -c2 * s3 B[1, 1] = 0 B[1, 2] = c2 * c3 B[2, 0] = s2 * c3 B[2, 1] = c2 B[2, 2] = s2 * s3 B = B / c2 return B
[docs]def BmatEuler212(q): """ BmatEuler212(Q) B = BmatEuler212(Q) returns the 3x3 matrix which relates the body angular velocity vector w to the derivative of (2-1-2) euler angle vector Q. dQ/dt = [B(Q)] w """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = s3 B[0, 1] = 0 B[0, 2] = -c3 B[1, 0] = s2 * c3 B[1, 1] = 0 B[1, 2] = s2 * s3 B[2, 0] = -c2 * s3 B[2, 1] = s2 B[2, 2] = c2 * c3 B = B / s2 return B
[docs]def BmatEuler213(q): """ BmatEuler213(Q) B = BmatEuler213(Q) returns the 3x3 matrix which relates the body angular velocity vector w to the derivative of (2-1-3) euler angle vector Q. dQ/dt = [B(Q)] w """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = s3 B[0, 1] = c3 B[0, 2] = 0 B[1, 0] = c2 * c3 B[1, 1] = -c2 * s3 B[1, 2] = 0 B[2, 0] = s2 * s3 B[2, 1] = s2 * c3 B[2, 2] = c2 B = B / c2 return B
[docs]def BmatEuler231(q): """ BmatEuler231(Q) B = BmatEuler231(Q) returns the 3x3 matrix which relates the body angular velocity vector w to the derivative of (2-3-1) euler angle vector Q. dQ/dt = [B(Q)] w """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = 0 B[0, 1] = c3 B[0, 2] = -s3 B[1, 0] = 0 B[1, 1] = c2 * s3 B[1, 2] = c2 * c3 B[2, 0] = c2 B[2, 1] = -s2 * c3 B[2, 2] = s2 * s3 B = B / c2 return B
[docs]def BmatEuler232(q): """ BmatEuler232(Q) B = BmatEuler232(Q) returns the 3x3 matrix which relates the body angular velocity vector w to the derivative of (2-3-2) euler angle vector Q. dQ/dt = [B(Q)] w """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = c3 B[0, 1] = 0 B[0, 2] = s3 B[1, 0] = -s2 * s3 B[1, 1] = 0 B[1, 2] = s2 * c3 B[2, 0] = -c2 * c3 B[2, 1] = s2 B[2, 2] = -c2 * s3 B = B / s2 return B
[docs]def BmatEuler312(q): """ BmatEuler312(Q) B = BmatEuler312(Q) returns the 3x3 matrix which relates the body angular velocity vector w to the derivative of (3-1-2) euler angle vector Q. dQ/dt = [B(Q)] w """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = -s3 B[0, 1] = 0 B[0, 2] = c3 B[1, 0] = c2 * c3 B[1, 1] = 0 B[1, 2] = c2 * s3 B[2, 0] = s2 * s3 B[2, 1] = c2 B[2, 2] = -s2 * c3 B = B / c2 return B
[docs]def BmatEuler313(q): """ BmatEuler313(Q) B = BmatEuler313(Q) returns the 3x3 matrix which relates the body angular velocity vector w to the derivative of (3-1-3) euler angle vector Q. dQ/dt = [B(Q)] w """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = s3 B[0, 1] = c3 B[0, 2] = 0 B[1, 0] = c3 * s2 B[1, 1] = -s3 * s2 B[1, 2] = 0 B[2, 0] = -s3 * c2 B[2, 1] = -c3 * c2 B[2, 2] = s2 B = B / s2 return B
[docs]def BmatEuler321(q): """ BmatEuler321(Q) B = BmatEuler321(Q) returns the 3x3 matrix which relates the body angular velocity vector w to the derivative of (3-2-1) euler angle vector Q. dQ/dt = [B(Q)] w """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = 0 B[0, 1] = s3 B[0, 2] = c3 B[1, 0] = 0 B[1, 1] = c2 * c3 B[1, 2] = -c2 * s3 B[2, 0] = c2 B[2, 1] = s2 * s3 B[2, 2] = s2 * c3 B = B / c2 return B
[docs]def BmatEuler323(q): """ BmatEuler323(Q) B = BmatEuler323(Q) returns the 3x3 matrix which relates the body angular velocity vector w to the derivative of (3-2-3) euler angle vector Q. dQ/dt = [B(Q)] w """ s2 = math.sin(q[1]) c2 = math.cos(q[1]) s3 = math.sin(q[2]) c3 = math.cos(q[2]) B = np.zeros([3, 3]) B[0, 0] = -c3 B[0, 1] = s3 B[0, 2] = 0 B[1, 0] = s2 * s3 B[1, 1] = s2 * c3 B[1, 2] = 0 B[2, 0] = c2 * c3 B[2, 1] = -c2 * s3 B[2, 2] = s2 B = B / s2 return B
[docs]def BmatGibbs(q): """ BmatGibbs(Q) B = BmatGibbs(Q) returns the 3x3 matrix which relates the body angular velocity vector w to the derivative of Gibbs vector Q. dQ/dt = 1/2 [B(Q)] w """ B = np.zeros([3, 3]) B[0, 0] = 1 + q[0] * q[0] B[0, 1] = q[0] * q[1] - q[2] B[0, 2] = q[0] * q[2] + q[1] B[1, 0] = q[1] * q[0] + q[2] B[1, 1] = 1 + q[1] * q[1] B[1, 2] = q[1] * q[2] - q[0] B[2, 0] = q[2] * q[0] - q[1] B[2, 1] = q[2] * q[1] + q[0] B[2, 2] = 1 + q[2] * q[2] return B
[docs]def BmatMRP(q): """ BmatMRP(Q) B = BmatMRP(Q) returns the 3x3 matrix which relates the body angular velocity vector w to the derivative of MRP vector Q. dQ/dt = 1/4 [B(Q)] w """ B = np.zeros([3, 3]) s2 = np.dot(q, q) B[0, 0] = 1 - s2 + 2 * q[0] * q[0] B[0, 1] = 2 * (q[0] * q[1] - q[2]) B[0, 2] = 2 * (q[0] * q[2] + q[1]) B[1, 0] = 2 * (q[1] * q[0] + q[2]) B[1, 1] = 1 - s2 + 2 * q[1] * q[1] B[1, 2] = 2 * (q[1] * q[2] - q[0]) B[2, 0] = 2 * (q[2] * q[0] - q[1]) B[2, 1] = 2 * (q[2] * q[1] + q[0]) B[2, 2] = 1 - s2 + 2 * q[2] * q[2] return B
[docs]def BdotmatMRP(q, dq): """ BdotmatMRP(Q, dQ) B = BdotmatMRP(Q, dQ) returns the derivative of the 3x3 BmatMRP matrix, which is used to calculate the second order derivative of the MRP vector Q. (d^2Q)/(dt^2) = 1/4 ( [B(Q)] dw + [Bdot(Q,dQ)] w ) """ Bdot = np.zeros([3, 3]) s = -2 * np.dot(q, dq) Bdot[0, 0] = s + 4 * (q[0] * dq[0]) Bdot[0, 1] = 2 * (-dq[2] + q[0] * dq[1] + dq[0] * q[1]) Bdot[0, 2] = 2 * ( dq[1] + q[0] * dq[2] + dq[0] * q[2]) Bdot[1, 0] = 2 * ( dq[2] + q[0] * dq[1] + dq[0] * q[1]) Bdot[1, 1] = s + 4 * (q[1] * dq[1]) Bdot[1, 2] = 2 * (-dq[0] + q[1] * dq[2] + dq[1] * q[2]) Bdot[2, 0] = 2 * (-dq[1] + q[0] * dq[2] + dq[0] * q[2]) Bdot[2, 1] = 2 * ( dq[0] + q[1] * dq[2] + dq[1] * q[2]) Bdot[2, 2] = s + 4 * (q[2] * dq[2]) return Bdot
[docs]def BmatPRV(q): """ BmatPRV(Q) B = BmatPRV(Q) returns the 3x3 matrix which relates the body angular velocity vector w to the derivative of principal rotation vector Q. dQ/dt = [B(Q)] w """ p = np.linalg.norm(q) c = 1 / p / p * (1 - p / 2 / math.tan(p / 2)) B = np.zeros([3, 3]) B[0, 0] = 1 - c * (q[1] * q[1] + q[2] * q[2]) B[0, 1] = -q[2] / 2 + c * (q[0] * q[1]) B[0, 2] = q[1] / 2 + c * (q[0] * q[2]) B[1, 0] = q[2] / 2 + c * (q[0] * q[1]) B[1, 1] = 1 - c * (q[0] * q[0] + q[2] * q[2]) B[1, 2] = -q[0] / 2 + c * (q[1] * q[2]) B[2, 0] = -q[1] / 2 + c * (q[0] * q[2]) B[2, 1] = q[0] / 2 + c * (q[1] * q[2]) B[2, 2] = 1 - c * (q[0] * q[0] + q[1] * q[1]) return B
[docs]def dEP(q, w): """ dEP(Q,W) dq = dEP(Q,W) returns the euler parameter derivative for a given euler parameter vector Q and body angular velocity vector w. dQ/dt = 1/2 [B(Q)] w """ return .5 * np.dot(BmatEP(q), w)
[docs]def dEuler121(q, w): """ dEuler121(Q,W) dq = dEuler121(Q,W) returns the (1-2-1) euler angle derivative vector for a given (1-2-1) euler angle vector Q and body angular velocity vector w. dQ/dt = [B(Q)] w """ return np.dot(BmatEuler121(q), w)
[docs]def dEuler123(q, w): """ dEuler123(Q,W) dq = dEuler123(Q,W) returns the (1-2-3) euler angle derivative vector for a given (1-2-3) euler angle vector Q and body angular velocity vector w. dQ/dt = [B(Q)] w """ return np.dot(BmatEuler123(q), w)
[docs]def dEuler131(q, w): """ dEuler131(Q,W) dq = dEuler131(Q,W) returns the (1-3-1) euler angle derivative vector for a given (1-3-1) euler angle vector Q and body angular velocity vector w. dQ/dt = [B(Q)] w """ return np.dot(BmatEuler131(q), w)
[docs]def dEuler132(q, w): """ dEuler132(Q,W) dq = dEuler132(Q,W) returns the (1-3-2) euler angle derivative vector for a given (1-3-2) euler angle vector Q and body angular velocity vector w. dQ/dt = [B(Q)] w """ return np.dot(BmatEuler132(q), w)
[docs]def dEuler212(q, w): """ dEuler212(Q,W) dq = dEuler212(Q,W) returns the (2-1-2) euler angle derivative vector for a given (2-1-2) euler angle vector Q and body angular velocity vector w. dQ/dt = [B(Q)] w """ return np.dot(BmatEuler212(q), w)
[docs]def dEuler213(q, w): """ dEuler213(Q,W) dq = dEuler213(Q,W) returns the (2-1-3) euler angle derivative vector for a given (2-1-3) euler angle vector Q and body angular velocity vector w. dQ/dt = [B(Q)] w """ return np.dot(BmatEuler213(q), w)
[docs]def dEuler231(q, w): """ dEuler231(Q,W) dq = dEuler231(Q,W) returns the (2-3-1) euler angle derivative vector for a given (2-3-1) euler angle vector Q and body angular velocity vector w. dQ/dt = [B(Q)] w """ return np.dot(BmatEuler231(q), w)
[docs]def dEuler232(q, w): """ dEuler232(Q,W) dq = dEuler232(Q,W) returns the (2-3-2) euler angle derivative vector for a given (2-3-2) euler angle vector Q and body angular velocity vector w. dQ/dt = [B(Q)] w """ return np.dot(BmatEuler232(q), w)
[docs]def dEuler312(q, w): """ dEuler312(Q,W) dq = dEuler312(Q,W) returns the (3-1-2) euler angle derivative vector for a given (3-1-2) euler angle vector Q and body angular velocity vector w. dQ/dt = [B(Q)] w """ return np.dot(BmatEuler312(q), w)
[docs]def dEuler313(q, w): """ dEuler313(Q,W) dq = dEuler313(Q,W) returns the (3-1-3) euler angle derivative vector for a given (3-1-3) euler angle vector Q and body angular velocity vector w. dQ/dt = [B(Q)] w """ return np.dot(BmatEuler313(q), w)
[docs]def dEuler321(q, w): """ dEuler321(Q,W) dq = dEuler321(Q,W) returns the (3-2-1) euler angle derivative vector for a given (3-2-1) euler angle vector Q and body angular velocity vector w. dQ/dt = [B(Q)] w """ return np.dot(BmatEuler321(q), w)
[docs]def dEuler323(q, w): """ dEuler323(Q,W) dq = dEuler323(Q,W) returns the (3-2-3) euler angle derivative vector for a given (3-2-3) euler angle vector Q and body angular velocity vector w. dQ/dt = [B(Q)] w """ return np.dot(BmatEuler323(q), w)
[docs]def dGibbs(q, w): """ dGibbs(Q,W) dq = dGibbs(Q,W) returns the gibbs derivative for a given gibbs vector Q and body angular velocity vector w. dQ/dt = 1/2 [B(Q)] w """ return .5 * np.dot(BmatGibbs(q), w)
[docs]def dMRP(q, w): """ dMRP(Q,W) dq = dMRP(Q,W) returns the MRP derivative for a given MRP vector Q and body angular velocity vector w. dQ/dt = 1/4 [B(Q)] w """ return .25 * np.dot(BmatMRP(q), w)
[docs]def dMRP2Omega(q, dq): """ dMRP(Q,dQ) W = dMRP(Q,dQ) returns the angular rate for a given MRP set q MRP derivative dq. W = 4 [B(Q)]^(-1) dQ """ return 4 * np.matmul(BinvMRP(q), dq)
[docs]def ddMRP(q, dq, w, dw): """ dMRP(Q,dQ,W,dW) ddQ = ddMRP(Q,dQ,W,dW) returns the MRP second derivative for a given MRP vector q, MRP derivative dq, body angular velocity vector w and body angulat acceleration vector dw. (d^2Q)/(dt^2) = 1/4 ( [B(Q)] dw + [Bdot(Q,dQ)] w ) """ return .25 * ( np.dot(BmatMRP(q), dw) + np.dot(BdotmatMRP(q, dq), w) )
[docs]def ddMRP2dOmega(q, dq, ddq): """ ddMRP2dOmega(Q,dQ,ddQ) dW = ddMRP2dOmega(Q,dQ,ddQ) returns the body angular acceleration dW given the MRP vector Q, the MRP derivative dQ and the MRP second order derivative ddQ. dW/dt = 4 [B(Q)]^(-1) ( ddQ - [Bdot(Q,dQ)] [B(Q)]^(-1) dQ ) """ Binv = BinvMRP(q) Bdot = BdotmatMRP(q, dq) return 4 * np.dot(Binv, (ddq - np.dot(Bdot, np.dot(Binv, dq))) )
[docs]def dPRV(q, w): """ dPRV(Q,W) dq = dPRV(Q,W) returns the PRV derivative for a given PRV vector Q and body angular velocity vector w. dQ/dt = [B(Q)] w """ return np.dot(BmatPRV(q), w)
[docs]def elem2PRV(r): """ elem2PRV(R) Q = elem2PRV(R) translates a prinicpal rotation element set R into the corresponding principal rotation vector Q. """ q0 = r[1] * r[0] q1 = r[2] * r[0] q2 = r[3] * r[0] q = np.array([q0, q1, q2]) return q
[docs]def gibbs2C(q): """ gibbs2C C = gibbs2C(Q) returns the direction cosine matrix in terms of the 3x1 gibbs vector Q. """ q1 = q[0] q2 = q[1] q3 = q[2] qm = np.linalg.norm(q) d1 = qm * qm C = np.zeros([3, 3]) C[0, 0] = 1 + 2 * q1 * q1 - d1 C[0, 1] = 2 * (q1 * q2 + q3) C[0, 2] = 2 * (q1 * q3 - q2) C[1, 0] = 2 * (q2 * q1 - q3) C[1, 1] = 1 + 2 * q2 * q2 - d1 C[1, 2] = 2 * (q2 * q3 + q1) C[2, 0] = 2 * (q3 * q1 + q2) C[2, 1] = 2 * (q3 * q2 - q1) C[2, 2] = 1 + 2 * q3 * q3 - d1 C = C / (1 + d1) return C
[docs]def gibbs2EP(q1): """ gibbs2EP(Q1) Q = gibbs2EP(Q1) translates the gibbs vector Q1 into the euler parameter vector Q. """ qm = np.linalg.norm(q1) ps = np.sqrt(1 + qm * qm) q = np.array([ 1 / ps, q1[0] / ps, q1[1] / ps, q1[2] / ps ]) return q
[docs]def gibbs2Euler121(q): """ gibbs2Euler121(Q) E = gibbs2Euler121(Q) translates the gibbs vector Q into the (1-2-1) euler angle vector E. """ return EP2Euler121(gibbs2EP(q))
[docs]def gibbs2Euler123(q): """ gibbs2Euler123(Q) E = gibbs2Euler123(Q) translates the gibbs vector Q into the (1-2-3) euler angle vector E. """ return EP2Euler123(gibbs2EP(q))
[docs]def gibbs2Euler131(q): """ gibbs2Euler131(Q) E = gibbs2Euler131(Q) translates the gibbs vector Q into the (1-3-1) euler angle vector E. """ return EP2Euler131(gibbs2EP(q))
[docs]def gibbs2Euler132(q): """ gibbs2Euler132(Q) E = gibbs2Euler132(Q) translates the gibbs vector Q into the (1-3-2) euler angle vector E. """ return EP2Euler132(gibbs2EP(q))
[docs]def gibbs2Euler212(q): """ gibbs2Euler212(Q) E = gibbs2Euler212(Q) translates the gibbs vector Q into the (2-1-2) euler angle vector E. """ return EP2Euler212(gibbs2EP(q))
[docs]def gibbs2Euler213(q): """ gibbs2Euler213(Q) E = gibbs2Euler213(Q) translates the gibbs vector Q into the (2-1-3) euler angle vector E. """ return EP2Euler213(gibbs2EP(q))
[docs]def gibbs2Euler231(q): """ gibbs2Euler231(Q) E = gibbs2Euler231(Q) translates the gibbs vector Q into the (2-3-1) euler angle vector E. """ return EP2Euler231(gibbs2EP(q))
[docs]def gibbs2Euler232(q): """ gibbs2Euler232(Q) E = gibbs2Euler232(Q) translates the gibbs vector Q into the (2-3-2) euler angle vector E. """ return EP2Euler232(gibbs2EP(q))
[docs]def gibbs2Euler312(q): """ gibbs2Euler312(Q) E = gibbs2Euler312(Q) translates the gibbs vector Q into the (3-1-2) euler angle vector E. """ return EP2Euler312(gibbs2EP(q))
[docs]def gibbs2Euler313(q): """ gibbs2Euler313(Q) E = gibbs2Euler313(Q) translates the gibbs vector Q into the (3-1-3) euler angle vector E. """ return EP2Euler313(gibbs2EP(q))
[docs]def gibbs2Euler321(q): """ gibbs2Euler321(Q) E = gibbs2Euler321(Q) translates the gibbs vector Q into the (3-2-1) euler angle vector E. """ return EP2Euler321(gibbs2EP(q))
[docs]def gibbs2Euler323(q): """ gibbs2Euler323(Q) E = gibbs2Euler323(Q) translates the gibbs vector Q into the (3-2-3) euler angle vector E. """ return EP2Euler323(gibbs2EP(q))
[docs]def gibbs2MRP(q1): """ gibbs2MRP(Q1) Q = gibbs2MRP(Q1) translates the gibbs vector Q1 into the MRP vector Q. """ return q1 / (1 + math.sqrt(1 + np.dot(q1, q1)))
[docs]def gibbs2PRV(q): """ gibbs2PRV(Q) Q = gibbs2PRV(Q1) translates the gibbs vector Q1 into the principal rotation vector Q. """ tp = np.linalg.norm(q) p = 2 * math.atan(tp) q0 = q[0] / tp * p q1 = q[1] / tp * p q2 = q[2] / tp * p q = np.array([q0, q1, q2]) return q
[docs]def MRP2C(q): """ MRP2C C = MRP2C(Q) returns the direction cosine matrix in terms of the 3x1 MRP vector Q. """ q1 = q[0] q2 = q[1] q3 = q[2] qm = np.linalg.norm(q) d1 = qm * qm S = 1 - d1 d = (1 + d1) * (1 + d1) C = np.zeros((3, 3)) C[0, 0] = 4 * (2 * q1 * q1 - d1) + S * S C[0, 1] = 8 * q1 * q2 + 4 * q3 * S C[0, 2] = 8 * q1 * q3 - 4 * q2 * S C[1, 0] = 8 * q2 * q1 - 4 * q3 * S C[1, 1] = 4 * (2 * q2 * q2 - d1) + S * S C[1, 2] = 8 * q2 * q3 + 4 * q1 * S C[2, 0] = 8 * q3 * q1 + 4 * q2 * S C[2, 1] = 8 * q3 * q2 - 4 * q1 * S C[2, 2] = 4 * (2 * q3 * q3 - d1) + S * S C = C / d return C
[docs]def MRP2EP(q1): """ MRP2EP(Q1) Q = MRP2EP(Q1) translates the MRP vector Q1 into the euler parameter vector Q. """ qm = np.linalg.norm(q1) ps = 1 + qm * qm q = np.array([ (1 - qm * qm) / ps, 2 * q1[0] / ps, 2 * q1[1] / ps, 2 * q1[2] / ps ]) return q
[docs]def MRP2Euler121(q): """ MRP2Euler121(Q) E = MRP2Euler121(Q) translates the MRP vector Q into the (1-2-1) euler angle vector E. """ return EP2Euler121(MRP2EP(q))
[docs]def MRP2Euler123(q): """ MRP2Euler123(Q) E = MRP2Euler123(Q) translates the MRP vector Q into the (1-2-3) euler angle vector E. """ return EP2Euler123(MRP2EP(q))
[docs]def MRP2Euler131(q): """ MRP2Euler131(Q) E = MRP2Euler131(Q) translates the MRP vector Q into the (1-3-1) euler angle vector E. """ return EP2Euler131(MRP2EP(q))
[docs]def MRP2Euler132(q): """ MRP2Euler132(Q) E = MRP2Euler132(Q) translates the MRP vector Q into the (1-3-2) euler angle vector E. """ return EP2Euler132(MRP2EP(q))
[docs]def MRP2Euler212(q): """ MRP2Euler212(Q) E = MRP2Euler212(Q) translates the MRP vector Q into the (2-1-2) euler angle vector E. """ return EP2Euler212(MRP2EP(q))
[docs]def MRP2Euler213(q): """ MRP2Euler213(Q) E = MRP2Euler213(Q) translates the MRP vector Q into the (2-1-3) euler angle vector E. """ return EP2Euler213(MRP2EP(q))
[docs]def MRP2Euler231(q): """ MRP2Euler231(Q) E = MRP2Euler231(Q) translates the MRP vector Q into the (2-3-1) euler angle vector E. """ return EP2Euler231(MRP2EP(q))
[docs]def MRP2Euler232(q): """ MRP2Euler232(Q) E = MRP2Euler232(Q) translates the MRP vector Q into the (2-3-2) euler angle vector E. """ return EP2Euler232(MRP2EP(q))
[docs]def MRP2Euler312(q): """ MRP2Euler312(Q) E = MRP2Euler312(Q) translates the MRP vector Q into the (3-1-2) euler angle vector E. """ return EP2Euler312(MRP2EP(q))
[docs]def MRP2Euler313(q): """ MRP2Euler313(Q) E = MRP2Euler313(Q) translates the MRP vector Q into the (3-1-3) euler angle vector E. """ return EP2Euler313(MRP2EP(q))
[docs]def MRP2Euler321(q): """ MRP2Euler321(Q) E = MRP2Euler321(Q) translates the MRP vector Q into the (3-2-1) euler angle vector E. """ return EP2Euler321(MRP2EP(q))
[docs]def MRP2Euler323(q): """ MRP2Euler323(Q) E = MRP2Euler323(Q) translates the MRP vector Q into the (3-2-3) euler angle vector E. """ return EP2Euler323(MRP2EP(q))
[docs]def MRP2Gibbs(q1): """ MRP2Gibbs(Q1) Q = MRP2Gibbs(Q1) translates the MRP vector Q1 into the gibbs vector Q. """ return 2 * q1 / (1 - np.dot(q1, q1))
[docs]def MRP2PRV(q): """ MRP2PRV(Q1) Q = MRP2PRV(Q1) translates the MRP vector Q1 into the principal rotation vector Q. """ tp = np.linalg.norm(q) p = 4 * math.atan(tp) q0 = q[0] / tp * p q1 = q[1] / tp * p q2 = q[2] / tp * p q = np.array([q0, q1, q2]) return q
[docs]def MRPswitch(q, s2): """ MRPswitch S = MRPswitch(Q,s2) checks to see if norm(Q) is larger than s2. If yes, then the MRP vector Q is mapped to its shadow set. """ q2 = np.dot(q, q) if (q2 > s2 * s2): s = -q / q2 else: s = q return s
[docs]def PRV2C(q): """ PRV2C C = PRV2C(Q) returns the direction cosine matrix in terms of the 3x1 principal rotation vector Q. """ q0 = np.linalg.norm(q) if q0 == 0.0: q1 = q[0] q2 = q[1] q3 = q[2] else: q1 = q[0] / q0 q2 = q[1] / q0 q3 = q[2] / q0 cp = np.cos(q0) sp = np.sin(q0) d1 = 1 - cp C = np.zeros((3, 3)) C[0, 0] = q1 * q1 * d1 + cp C[0, 1] = q1 * q2 * d1 + q3 * sp C[0, 2] = q1 * q3 * d1 - q2 * sp C[1, 0] = q2 * q1 * d1 - q3 * sp C[1, 1] = q2 * q2 * d1 + cp C[1, 2] = q2 * q3 * d1 + q1 * sp C[2, 0] = q3 * q1 * d1 + q2 * sp C[2, 1] = q3 * q2 * d1 - q1 * sp C[2, 2] = q3 * q3 * d1 + cp return C
[docs]def PRV2EP(qq1): """" PRV2EP(Q1) Q = PRV2EP(Q1) translates the principal rotation vector Q1 into the euler parameter vector Q. """ q = np.zeros(4) q1 = PRV2elem(qq1) sp = math.sin(q1[0] / 2) q[0] = math.cos(q1[0] / 2) q[1] = q1[1] * sp q[2] = q1[2] * sp q[3] = q1[3] * sp return q
[docs]def PRV2Euler121(q): """ PRV2Euler121(Q) E = PRV2Euler121(Q) translates the principal rotation vector Q into the (1-2-1) euler angle vector E. """ return EP2Euler121(PRV2EP(q))
[docs]def PRV2Euler123(q): """ PRV2Euler123(Q) E = PRV2Euler123(Q) translates the principal rotation vector Q into the (1-2-3) euler angle vector E. """ return EP2Euler123(PRV2EP(q))
[docs]def PRV2Euler131(q): """ PRV2Euler131(Q) E = PRV2Euler131(Q) translates the principal rotation vector Q into the (1-3-1) euler angle vector E. """ return EP2Euler131(PRV2EP(q))
[docs]def PRV2Euler132(q): """ PRV2Euler132(Q) E = PRV2Euler132(Q) translates the principal rotation vector Q into the (1-3-2) euler angle vector E. """ return EP2Euler132(PRV2EP(q))
[docs]def PRV2Euler212(q): """ PRV2Euler212(Q) E = PRV2Euler212(Q) translates the principal rotation vector Q into the (2-1-2) euler angle vector E. """ return EP2Euler212(PRV2EP(q))
[docs]def PRV2Euler213(q): """ PRV2Euler213(Q) E = PRV2Euler213(Q) translates the principal rotation vector Q into the (2-1-3) euler angle vector E. """ return EP2Euler213(PRV2EP(q))
[docs]def PRV2Euler231(q): """ PRV2Euler231(Q) E = PRV2Euler231(Q) translates the principal rotation vector Q into the (2-3-1) euler angle vector E. """ return EP2Euler231(PRV2EP(q))
[docs]def PRV2Euler232(q): """ PRV2Euler232(Q) E = PRV2Euler232(Q) translates the principal rotation vector Q into the (2-3-2) euler angle vector E. """ return EP2Euler232(PRV2EP(q))
[docs]def PRV2Euler312(q): """ PRV2Euler312(Q) E = PRV2Euler312(Q) translates the principal rotation vector Q into the (3-1-2) euler angle vector E. """ return EP2Euler312(PRV2EP(q))
[docs]def PRV2Euler313(q): """ PRV2Euler313(Q) E = PRV2Euler313(Q) translates the principal rotation vector Q into the (3-1-3) euler angle vector E. """ return EP2Euler313(PRV2EP(q))
[docs]def PRV2Euler321(q): """ PRV2Euler321(Q) E = PRV2Euler321(Q) translates the principal rotation vector Q into the (3-2-1) euler angle vector E. """ return EP2Euler321(PRV2EP(q))
[docs]def PRV2Euler323(q): """ PRV2Euler323(Q) E = PRV2Euler323(Q) translates the principal rotation vector Q into the (3-2-3) euler angle vector E. """ return EP2Euler323(PRV2EP(q))
[docs]def PRV2Gibbs(q): """ PRV2Gibbs(Q1) Q = PRV2Gibbs(Q1) translates the principal rotation vector Q1 into the gibbs vector Q. """ q = PRV2elem(q) tp = math.tan(q[0] / 2) q0 = q[1] * tp q1 = q[2] * tp q2 = q[3] * tp q = np.array([q0, q1, q2]) return q
[docs]def PRV2MRP(q): """ PRV2MRP(Q1) Q = PRV2MRP(Q1) translates the principal rotation vector Q1 into the MRP vector Q. """ q = PRV2elem(q) tp = math.tan(q[0] / 4) q0 = q[1] * tp q1 = q[2] * tp q2 = q[3] * tp q = np.array([q0, q1, q2]) return q
[docs]def subEP(b1, b2): """ subEP(B1,B2) Q = subEP(B1,B2) provides the euler parameter vector which corresponds to relative rotation from B2 to B1. """ q = np.zeros(4) q[0] = b2[0] * b1[0] + b2[1] * b1[1] + b2[2] * b1[2] + b2[3] * b1[3] q[1] = -b2[1] * b1[0] + b2[0] * b1[1] + b2[3] * b1[2] - b2[2] * b1[3] q[2] = -b2[2] * b1[0] - b2[3] * b1[1] + b2[0] * b1[2] + b2[1] * b1[3] q[3] = -b2[3] * b1[0] + b2[2] * b1[1] - b2[1] * b1[2] + b2[0] * b1[3] return q
[docs]def subEuler121(e, e1): """ subEuler121(E,E1) E2 = subEuler121(E,E1) computes the relative (1-2-1) euler angle vector from E1 to E. """ cp = math.cos(e[1]) cp1 = math.cos(e1[1]) sp = math.sin(e[1]) sp1 = math.sin(e1[1]) dum = e[0] - e1[0] e2 = np.zeros(3) e2[1] = math.acos(cp1 * cp + sp1 * sp * math.cos(dum)) cp2 = math.cos(e2[1]) e2[0] = Picheck(-e1[2] + math.atan2(sp1 * sp * math.sin(dum), cp2 * cp1 - cp)) e2[2] = Picheck(e[2] - math.atan2(sp1 * sp * math.sin(dum), cp1 - cp * cp2)) return e2
[docs]def subEuler123(e, e1): """ subEuler123(E,E1) E2 = subEuler123(E,E1) computes the relative (1-2-3) euler angle vector from E1 to E. """ C = euler1232C(e) C1 = euler1232C(e1) C2 = np.dot(C, C1.T) e2 = C2Euler123(C2) return e2
[docs]def subEuler131(e, e1): """ subEuler131(E,E1) E2 = subEuler131(E,E1) computes the relative (1-3-1) euler angle vector from E1 to E. """ cp = math.cos(e[1]) cp1 = math.cos(e1[1]) sp = math.sin(e[1]) sp1 = math.sin(e1[1]) dum = e[0] - e1[0] e2 = np.zeros(3) e2[1] = math.acos(cp1 * cp + sp1 * sp * math.cos(dum)) cp2 = math.cos(e2[1]) e2[0] = Picheck(-e1[2] + math.atan2(sp1 * sp * math.sin(dum), cp2 * cp1 - cp)) e2[2] = Picheck(e[2] - math.atan2(sp1 * sp * math.sin(dum), cp1 - cp * cp2)) return e2
[docs]def subEuler132(e, e1): """ subEuler132(E,E1) E2 = subEuler132(E,E1) computes the relative (1-3-2) euler angle vector from E1 to E. """ C = euler1322C(e) C1 = euler1322C(e1) C2 = np.dot(C, C1.T) e2 = C2Euler132(C2) return e2
[docs]def subEuler212(e, e1): """ subEuler212(E,E1) E2 = subEuler212(E,E1) computes the relative (2-1-2) euler angle vector from E1 to E. """ cp = math.cos(e[1]) cp1 = math.cos(e1[1]) sp = math.sin(e[1]) sp1 = math.sin(e1[1]) dum = e[0] - e1[0] e2 = np.zeros(3) e2[1] = math.acos(cp1 * cp + sp1 * sp * math.cos(dum)) cp2 = math.cos(e2[1]) e2[0] = Picheck(-e1[2] + math.atan2(sp1 * sp * math.sin(dum), cp2 * cp1 - cp)) e2[2] = Picheck(e[2] - math.atan2(sp1 * sp * math.sin(dum), cp1 - cp * cp2)) return e2
[docs]def subEuler213(e, e1): """ subEuler213(E,E1) E2 = subEuler213(E,E1) computes the relative (2-1-3) euler angle vector from E1 to E. """ C = euler2132C(e) C1 = euler2132C(e1) C2 = np.dot(C, C1.T) e2 = C2Euler213(C2) return e2
[docs]def subEuler231(e, e1): """ subEuler231(E,E1) E2 = subEuler231(E,E1) computes the relative (2-3-1) euler angle vector from E1 to E. """ C = euler2312C(e) C1 = euler2312C(e1) C2 = np.dot(C, C1.T) e2 = C2Euler231(C2) return e2
[docs]def subEuler232(e, e1): """ subEuler232(E,E1) E2 = subEuler232(E,E1) computes the relative (2-3-2) euler angle vector from E1 to E. """ cp = math.cos(e[1]) cp1 = math.cos(e1[1]) sp = math.sin(e[1]) sp1 = math.sin(e1[1]) dum = e[0] - e1[0] e2 = np.zeros(3) e2[1] = math.acos(cp1 * cp + sp1 * sp * math.cos(dum)) cp2 = math.cos(e2[1]) e2[0] = Picheck(-e1[2] + math.atan2(sp1 * sp * math.sin(dum), cp2 * cp1 - cp)) e2[2] = Picheck(e[2] - math.atan2(sp1 * sp * math.sin(dum), cp1 - cp * cp2)) return e2
[docs]def subEuler312(e, e1): """ subEuler312(E,E1) E2 = subEuler312(E,E1) computes the relative (3-1-2) euler angle vector from E1 to E. """ C = euler3122C(e) C1 = euler3122C(e1) C2 = np.dot(C, C1.T) e2 = C2Euler312(C2) return e2
[docs]def subEuler313(e, e1): """ subEuler313(E,E1) E2 = subEuler313(E,E1) computes the relative (3-1-3) euler angle vector from E1 to E. """ cp = math.cos(e[1]) cp1 = math.cos(e1[1]) sp = math.sin(e[1]) sp1 = math.sin(e1[1]) dum = e[0] - e1[0] e2 = np.zeros(3) e2[1] = math.acos(cp1 * cp + sp1 * sp * math.cos(dum)) cp2 = math.cos(e2[1]) e2[0] = Picheck(-e1[2] + math.atan2(sp1 * sp * math.sin(dum), cp2 * cp1 - cp)) e2[2] = Picheck(e[2] - math.atan2(sp1 * sp * math.sin(dum), cp1 - cp * cp2)) return e2
[docs]def subEuler321(e, e1): """ subEuler321(E,E1) E2 = subEuler321(E,E1) computes the relative (3-2-1) euler angle vector from E1 to E. """ C = euler3212C(e) C1 = euler3212C(e1) C2 = np.dot(C, C1.T) e2 = C2Euler321(C2) return e2
[docs]def subEuler323(e, e1): """ subEuler323(E,E1) E2 = subEuler323(E,E1) computes the relative (3-2-3) euler angle vector from E1 to E. """ cp = math.cos(e[1]) cp1 = math.cos(e1[1]) sp = math.sin(e[1]) sp1 = math.sin(e1[1]) dum = e[0] - e1[0] e2 = np.zeros(3) e2[1] = math.acos(cp1 * cp + sp1 * sp * math.cos(dum)) cp2 = math.cos(e2[1]) e2[0] = Picheck(-e1[2] + math.atan2(sp1 * sp * math.sin(dum), cp2 * cp1 - cp)) e2[2] = Picheck(e[2] - math.atan2(sp1 * sp * math.sin(dum), cp1 - cp * cp2)) return e2
[docs]def subGibbs(q1, q2): """ subGibbs(Q1,Q2) Q = subGibbs(Q1,Q2) provides the gibbs vector which corresponds to relative rotation from Q2 to Q1. """ return (q1 - q2 + np.cross(q1, q2)) / (1. + np.dot(q1, q2))
[docs]def subMRP(q1, q2): """ subMRP(Q1,Q2) Q = subMRP(Q1,Q2) provides the MRP vector which corresponds to relative rotation from Q2 to Q1. """ q2m = np.linalg.norm(q2) q1m = np.linalg.norm(q1) den = 1 + (q1m * q1m) * (q2m * q2m) + 2 * np.dot(q1, q2) if den < 1e-5: q2 = -q2/np.dot(q2,q2) den = 1 + (q1m * q1m) * (q2m * q2m) + 2 * np.dot(q1, q2) num = (1 - q2m * q2m) * q1 - (1 - q1m * q1m) * q2 + 2 * np.cross(q1, q2) q = num / den if np.dot(q,q) > 1: q = -q/np.dot(q, q) return q
[docs]def subPRV(q1, q2): """ subPRV(Q1,Q2) Q = subPRV(Q1,Q2) provides the prinipal rotation vector which corresponds to relative principal rotation from Q2 to Q1. """ q1 = PRV2elem(q1) q2 = PRV2elem(q2) cp1 = math.cos(q1[0] / 2) cp2 = math.cos(q2[0] / 2) sp1 = math.sin(q1[0] / 2) sp2 = math.sin(q2[0] / 2) e1 = q1[1:4] e2 = q2[1:4] p = 2 * math.acos(cp1 * cp2 + sp1 * sp2 * np.dot(e1, e2)) sp = math.sin(p / 2) e = (-cp1 * sp2 * e2 + cp2 * sp1 * e1 + sp1 * sp2 * np.cross(e1, e2)) / sp q = p * e return q
[docs]def EP2C(q): """ EP2C C = EP2C(Q) returns the direction math.cosine matrix in terms of the 4x1 euler parameter vector Q. The first element is the non-dimensional euler parameter, while the remain three elements form the eulerparameter vector. """ q0 = q[0] q1 = q[1] q2 = q[2] q3 = q[3] C = np.zeros([3, 3]) C[0, 0] = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3 C[0, 1] = 2 * (q1 * q2 + q0 * q3) C[0, 2] = 2 * (q1 * q3 - q0 * q2) C[1, 0] = 2 * (q1 * q2 - q0 * q3) C[1, 1] = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3 C[1, 2] = 2 * (q2 * q3 + q0 * q1) C[2, 0] = 2 * (q1 * q3 + q0 * q2) C[2, 1] = 2 * (q2 * q3 - q0 * q1) C[2, 2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3 return C
[docs]def EP2Euler121(q): """ EP2Euler121(Q) E = EP2Euler121(Q) translates the euler parameter vector Q into the corresponding (1-2-1) euler angle vector E. """ t1 = math.atan2(q[3], q[2]) t2 = math.atan2(q[1], q[0]) e1 = t1 + t2 e2 = 2 * math.acos(math.sqrt(q[0] * q[0] + q[1] * q[1])) e3 = t2 - t1 e = np.array([e1, e2, e3]) return e
[docs]def EP2Euler123(q): """ EP2Euler123 Q = EP2Euler123(Q) translates the euler parameter vector Q into the corresponding (1-2-3) euler angle set. """ q0 = q[0] q1 = q[1] q2 = q[2] q3 = q[3] e1 = math.atan2(-2 * (q2 * q3 - q0 * q1), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) e2 = math.asin(2 * (q1 * q3 + q0 * q2)) e3 = math.atan2(-2 * (q1 * q2 - q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) e = np.array([e1, e2, e3]) return e
[docs]def EP2Euler131(q): """ EP2Euler131(Q) E = EP2Euler131(Q) translates the euler parameter vector Q into the corresponding (1-3-1) euler angle vector E. """ t1 = math.atan2(q[2], q[3]) t2 = math.atan2(q[1], q[0]) e1 = t2 - t1 e2 = 2 * math.acos(math.sqrt(q[0] * q[0] + q[1] * q[1])) e3 = t2 + t1 e = np.array([e1, e2, e3]) return e
[docs]def EP2Euler132(q): """ EP2Euler132 E = EP2Euler132(Q) translates the euler parameter vector Q into the corresponding (1-3-2) euler angle set. """ q0 = q[0] q1 = q[1] q2 = q[2] q3 = q[3] e1 = math.atan2(2 * (q2 * q3 + q0 * q1), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3) e2 = math.asin(-2 * (q1 * q2 - q0 * q3)) e3 = math.atan2(2 * (q1 * q3 + q0 * q2), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) e = np.array([e1, e2, e3]) return e
[docs]def EP2Euler212(q): """ EP2Euler212(Q) E = EP2Euler212(Q) translates the euler parameter vector Q into the corresponding (2-1-2) euler angle vector E. """ t1 = math.atan2(q[3], q[1]) t2 = math.atan2(q[2], q[0]) e1 = t2 - t1 e2 = 2 * math.acos(math.sqrt(q[0] * q[0] + q[2] * q[2])) e3 = t2 + t1 e = np.array([e1, e2, e3]) return e
[docs]def EP2Euler213(q): """ EP2Euler213 Q = EP2Euler213(Q) translates the euler parameter vector Q into the corresponding (2-1-3) euler angle set. """ q0 = q[0] q1 = q[1] q2 = q[2] q3 = q[3] e1 = math.atan2(2 * (q1 * q3 + q0 * q2), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) e2 = math.asin(-2 * (q2 * q3 - q0 * q1)) e3 = math.atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3) e = np.array([e1, e2, e3]) return e
[docs]def EP2Euler231(q): """ EP2Euler231 E = EP2Euler231(Q) translates the euler parameter vector Q into the corresponding (2-3-1) euler angle set. """ q0 = q[0] q1 = q[1] q2 = q[2] q3 = q[3] e1 = math.atan2(-2 * (q1 * q3 - q0 * q2), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) e2 = math.asin(2 * (q1 * q2 + q0 * q3)) e3 = math.atan2(-2 * (q2 * q3 - q0 * q1), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3) e = np.array([e1, e2, e3]) return e
[docs]def EP2Euler232(q): """ EP2Euler232(Q) E = EP2Euler232(Q) translates the euler parameter vector Q into the corresponding (2-3-2) euler angle vector E. """ t1 = math.atan2(q[1], q[3]) t2 = math.atan2(q[2], q[0]) e1 = t1 + t2 e2 = 2 * math.acos(math.sqrt(q[0] * q[0] + q[2] * q[2])) e3 = t2 - t1 e = np.array([e1, e2, e3]) return e
[docs]def EP2Euler312(q): """ EP2Euler312 E = EP2Euler312(Q) translates the euler parameter vector Q into the corresponding (3-1-2) euler angle set. """ q0 = q[0] q1 = q[1] q2 = q[2] q3 = q[3] e1 = math.atan2(-2 * (q1 * q2 - q0 * q3), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3) e2 = math.asin(2 * (q2 * q3 + q0 * q1)) e3 = math.atan2(-2 * (q1 * q3 - q0 * q2), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) e = np.array([e1, e2, e3]) return e
[docs]def EP2Euler313(q): """ EP2Euler313(Q) E = EP2Euler313(Q) translates the euler parameter vector Q into the corresponding (3-1-3) euler angle vector E. """ t1 = math.atan2(q[2], q[1]) t2 = math.atan2(q[3], q[0]) e1 = t1 + t2 e2 = 2 * math.acos(math.sqrt(q[0] * q[0] + q[3] * q[3])) e3 = t2 - t1 e = np.array([e1, e2, e3]) return e
[docs]def EP2Euler321(q): """ EP2Euler321 E = EP2Euler321(Q) translates the euler parameter vector Q into the corresponding (3-2-1) euler angle set. """ q0 = q[0] q1 = q[1] q2 = q[2] q3 = q[3] e1 = math.atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) e2 = math.asin(-2 * (q1 * q3 - q0 * q2)) e3 = math.atan2(2 * (q2 * q3 + q0 * q1), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) e = np.array([e1, e2, e3]) return e
[docs]def EP2Euler323(q): """ EP2Euler323(Q) E = EP2Euler323(Q) translates the euler parameter vector Q into the corresponding (3-2-3) euler angle vector E. """ t1 = math.atan2(q[1], q[2]) t2 = math.atan2(q[3], q[0]) e1 = t2 - t1 e2 = 2 * math.acos(math.sqrt(q[0] * q[0] + q[3] * q[3])) e3 = t2 + t1 e = np.array([e1, e2, e3]) return e
[docs]def EP2Gibbs(q): """ EP2Gibbs(Q1) Q = EP2Gibbs(Q1) translates the euler parameter vector Q1 into the gibbs vector Q. """ q1 = q[1] / q[0] q2 = q[2] / q[0] q3 = q[3] / q[0] return np.array([q1, q2, q3])
[docs]def EP2MRP(q): """ EP2MRP(Q1) Q = EP2MRP(Q1) translates the euler parameter vector Q1 into the MRP vector Q. """ if q[0] < 0: q = -q q1 = q[1] / (1 + q[0]) q2 = q[2] / (1 + q[0]) q3 = q[3] / (1 + q[0]) return np.array([q1, q2, q3])
[docs]def EP2PRV(q): """ EP2PRV(Q1) Q = EP2PRV(Q1) translates the euler parameter vector Q1 into the principal rotation vector Q. """ p = 2 * math.acos(q[0]) sp = math.sin(p / 2) q1 = q[1] / sp * p q2 = q[2] / sp * p q3 = q[3] / sp * p return np.array([q1, q2, q3])
[docs]def euler1(x): """ EULER1 Elementary rotation matrix Returns the elementary rotation matrix about the first body axis. """ m = np.identity(3) m[1, 1] = math.cos(x) m[1, 2] = math.sin(x) m[2, 1] = -m[1, 2] m[2, 2] = m[1, 1] return m
[docs]def euler2(x): """ EULER2 Elementary rotation matrix Returns the elementary rotation matrix about the second body axis. """ m = np.identity(3) m[0, 0] = math.cos(x) m[0, 2] = -math.sin(x) m[2, 0] = -m[0, 2] m[2, 2] = m[0, 0] return m
[docs]def euler3(x): """ EULER3 Elementary rotation matrix Returns the elementary rotation matrix about the third body axis. """ m = np.identity(3) m[0, 0] = math.cos(x) m[0, 1] = math.sin(x) m[1, 0] = -m[0, 1] m[1, 1] = m[0, 0] return m
[docs]def euler1212C(q): """ Euler1212C C = euler1212C(Q) returns the direction cosine matrix in terms of the 1-2-1 euler angles. Input Q must be a 3x1 vector of euler angles. """ st1 = math.sin(q[0]) ct1 = math.cos(q[0]) st2 = math.sin(q[1]) ct2 = math.cos(q[1]) st3 = math.sin(q[2]) ct3 = math.cos(q[2]) C = np.identity(3) C[0, 0] = ct2 C[0, 1] = st1 * st2 C[0, 2] = -ct1 * st2 C[1, 0] = st2 * st3 C[1, 1] = ct1 * ct3 - ct2 * st1 * st3 C[1, 2] = ct3 * st1 + ct1 * ct2 * st3 C[2, 0] = ct3 * st2 C[2, 1] = -ct2 * ct3 * st1 - ct1 * st3 C[2, 2] = ct1 * ct2 * ct3 - st1 * st3 return C
[docs]def euler1212EP(e): """ Euler1212EP(E) Q = euler1212EP(E) translates the 121 euler angle vector E into the euler parameter vector Q. """ e1 = e[0] / 2 e2 = e[1] / 2 e3 = e[2] / 2 q0 = math.cos(e2) * math.cos(e1 + e3) q1 = math.cos(e2) * math.sin(e1 + e3) q2 = math.sin(e2) * math.cos(e1 - e3) q3 = math.sin(e2) * math.sin(e1 - e3) return np.array([q0, q1, q2, q3])
[docs]def euler1212Gibbs(e): """ Euler1212Gibbs(E) Q = euler1212Gibbs(E) translates the (1-2-1) euler angle vector E into the gibbs vector Q. """ return EP2Gibbs(euler1212EP(e))
[docs]def euler1212MRP(e): """ euler1212MRP(E) Q = euler1212MRP(E) translates the (1-2-1) euler angle vector E into the MRP vector Q. """ return EP2MRP(euler1212EP(e))
[docs]def euler1212PRV(e): """ euler1212PRV(E) Q = euler1212PRV(E) translates the (1-2-1) euler angle vector E into the principal rotation vector Q. """ return EP2PRV(euler1212EP(e))
[docs]def euler1232C(q): """ euler1232C C = euler1232C(Q) returns the direction cosine matrix in terms of the 1-2-3 euler angles. Input Q must be a 3x1 vector of euler angles. """ st1 = math.sin(q[0]) ct1 = math.cos(q[0]) st2 = math.sin(q[1]) ct2 = math.cos(q[1]) st3 = math.sin(q[2]) ct3 = math.cos(q[2]) C = np.identity(3) C[0, 0] = ct2 * ct3 C[0, 1] = ct3 * st1 * st2 + ct1 * st3 C[0, 2] = st1 * st3 - ct1 * ct3 * st2 C[1, 0] = -ct2 * st3 C[1, 1] = ct1 * ct3 - st1 * st2 * st3 C[1, 2] = ct3 * st1 + ct1 * st2 * st3 C[2, 0] = st2 C[2, 1] = -ct2 * st1 C[2, 2] = ct1 * ct2 return C
[docs]def euler1232EP(e): """ euler1232EP(E) Q = euler1232EP(E) translates the 123 euler angle vector E into the euler parameter vector Q. """ c1 = math.cos(e[0] / 2) s1 = math.sin(e[0] / 2) c2 = math.cos(e[1] / 2) s2 = math.sin(e[1] / 2) c3 = math.cos(e[2] / 2) s3 = math.sin(e[2] / 2) q0 = c1 * c2 * c3 - s1 * s2 * s3 q1 = s1 * c2 * c3 + c1 * s2 * s3 q2 = c1 * s2 * c3 - s1 * c2 * s3 q3 = c1 * c2 * s3 + s1 * s2 * c3 return np.array([q0, q1, q2, q3])
[docs]def euler1232Gibbs(e): """ euler1232Gibbs(E) Q = euler1232Gibbs(E) translates the (1-2-3) euler angle vector E into the gibbs vector Q. """ return EP2Gibbs(euler1232EP(e))
[docs]def euler1232MRP(e): """ euler1232MRP(E) Q = euler1232MRP(E) translates the (1-2-3) euler angle vector E into the MRP vector Q. """ return EP2MRP(euler1232EP(e))
[docs]def euler1232PRV(e): """ euler1232PRV(E) Q = euler1232PRV(E) translates the (1-2-3) euler angle vector E into the principal rotation vector Q. """ return EP2PRV(euler1232EP(e))
[docs]def euler1312C(q): """ euler1312C C = euler1312C(Q) returns the direction cosine matrix in terms of the 1-3-1 euler angles. Input Q must be a 3x1 vector of euler angles. """ st1 = math.sin(q[0]) ct1 = math.cos(q[0]) st2 = math.sin(q[1]) ct2 = math.cos(q[1]) st3 = math.sin(q[2]) ct3 = math.cos(q[2]) C = np.identity(3) C[0, 0] = ct2 C[0, 1] = ct1 * st2 C[0, 2] = st1 * st2 C[1, 0] = -ct3 * st2 C[1, 1] = ct1 * ct2 * ct3 - st1 * st3 C[1, 2] = ct2 * ct3 * st1 + ct1 * st3 C[2, 0] = st2 * st3 C[2, 1] = -ct3 * st1 - ct1 * ct2 * st3 C[2, 2] = ct1 * ct3 - ct2 * st1 * st3 return C
[docs]def euler1312EP(e): """ euler1312EP(E) Q = euler1312EP(E) translates the 131 euler angle vector E into the euler parameter vector Q. """ e1 = e[0] / 2 e2 = e[1] / 2 e3 = e[2] / 2 q0 = math.cos(e2) * math.cos(e1 + e3) q1 = math.cos(e2) * math.sin(e1 + e3) q2 = math.sin(e2) * math.sin(-e1 + e3) q3 = math.sin(e2) * math.cos(-e1 + e3) return np.array([q0, q1, q2, q3])
[docs]def euler1312Gibbs(e): """ euler1312Gibbs(E) Q = euler1312Gibbs(E) translates the (1-3-1) euler angle vector E into the gibbs vector Q. """ return EP2Gibbs(euler1312EP(e))
[docs]def euler1312MRP(e): """ euler1312MRP(E) Q = euler1312MRP(E) translates the (1-3-1) euler angle vector E into the MRP vector Q. """ return EP2MRP(euler1312EP(e))
[docs]def euler1312PRV(e): """ euler1312PRV(E) Q = euler1312PRV(E) translates the (1-3-1) euler angle vector E into the principal rotation vector Q. """ return EP2PRV(euler1312EP(e))
[docs]def euler1322C(q): """ euler1322C C = euler1322C(Q) returns the direction cosine matrix in terms of the 1-3-2 euler angles. Input Q must be a 3x1 vector of euler angles. """ st1 = math.sin(q[0]) ct1 = math.cos(q[0]) st2 = math.sin(q[1]) ct2 = math.cos(q[1]) st3 = math.sin(q[2]) ct3 = math.cos(q[2]) C = np.identity(3) C[0, 0] = ct2 * ct3 C[0, 1] = ct1 * ct3 * st2 + st1 * st3 C[0, 2] = ct3 * st1 * st2 - ct1 * st3 C[1, 0] = -st2 C[1, 1] = ct1 * ct2 C[1, 2] = ct2 * st1 C[2, 0] = ct2 * st3 C[2, 1] = -ct3 * st1 + ct1 * st2 * st3 C[2, 2] = ct1 * ct3 + st1 * st2 * st3 return C
[docs]def euler1322EP(e): """ euler1322EP(E) Q = euler1322EP(E) translates the 132 euler angle vector E into the euler parameter vector Q. """ c1 = math.cos(e[0] / 2) s1 = math.sin(e[0] / 2) c2 = math.cos(e[1] / 2) s2 = math.sin(e[1] / 2) c3 = math.cos(e[2] / 2) s3 = math.sin(e[2] / 2) q0 = c1 * c2 * c3 + s1 * s2 * s3 q1 = s1 * c2 * c3 - c1 * s2 * s3 q2 = c1 * c2 * s3 - s1 * s2 * c3 q3 = c1 * s2 * c3 + s1 * c2 * s3 return np.array([q0, q1, q2, q3])
[docs]def euler1322Gibbs(e): """ euler1322Gibbs(E) Q = euler1322Gibbs(E) translates the (1-3-2) euler angle vector E into the gibbs vector Q. """ return EP2Gibbs(euler1322EP(e))
[docs]def euler1322MRP(e): """ euler1322MRP(E) Q = euler1322MRP(E) translates the (1-3-2) euler angle vector E into the MRP vector Q. """ return EP2MRP(euler1322EP(e))
[docs]def euler1322PRV(e): """ euler1322PRV(E) Q = euler1322PRV(E) translates the (1-3-2) euler angle vector E into the principal rotation vector Q. """ return EP2PRV(euler1322EP(e))
[docs]def euler2122C(q): """ euler2122C C = euler2122C(Q) returns the direction cosine matrix in terms of the 2-1-2 euler angles. Input Q must be a 3x1 vector of euler angles. """ st1 = math.sin(q[0]) ct1 = math.cos(q[0]) st2 = math.sin(q[1]) ct2 = math.cos(q[1]) st3 = math.sin(q[2]) ct3 = math.cos(q[2]) C = np.identity(3) C[0, 0] = ct1 * ct3 - ct2 * st1 * st3 C[0, 1] = st2 * st3 C[0, 2] = -ct3 * st1 - ct1 * ct2 * st3 C[1, 0] = st1 * st2 C[1, 1] = ct2 C[1, 2] = ct1 * st2 C[2, 0] = ct2 * ct3 * st1 + ct1 * st3 C[2, 1] = -ct3 * st2 C[2, 2] = ct1 * ct2 * ct3 - st1 * st3 return C
[docs]def euler2132C(q): """ euler2132C C = euler2132C(Q) returns the direction cosine matrix in terms of the 2-1-3 euler angles. Input Q must be a 3x1 vector of euler angles. """ st1 = math.sin(q[0]) ct1 = math.cos(q[0]) st2 = math.sin(q[1]) ct2 = math.cos(q[1]) st3 = math.sin(q[2]) ct3 = math.cos(q[2]) C = np.identity(3) C[0, 0] = ct1 * ct3 + st1 * st2 * st3 C[0, 1] = ct2 * st3 C[0, 2] = -ct3 * st1 + ct1 * st2 * st3 C[1, 0] = ct3 * st1 * st2 - ct1 * st3 C[1, 1] = ct2 * ct3 C[1, 2] = ct1 * ct3 * st2 + st1 * st3 C[2, 0] = ct2 * st1 C[2, 1] = -st2 C[2, 2] = ct1 * ct2 return C
[docs]def euler2312C(q): """ euler2312C C = euler2312C(Q) returns the direction cosine matrix in terms of the 2-3-1 euler angles. Input Q must be a 3x1 vector of euler angles. """ st1 = math.sin(q[0]) ct1 = math.cos(q[0]) st2 = math.sin(q[1]) ct2 = math.cos(q[1]) st3 = math.sin(q[2]) ct3 = math.cos(q[2]) C = np.identity(3) C[0, 0] = ct1 * ct2 C[0, 1] = st2 C[0, 2] = -ct2 * st1 C[1, 0] = -ct1 * ct3 * st2 + st1 * st3 C[1, 1] = ct2 * ct3 C[1, 2] = ct3 * st1 * st2 + ct1 * st3 C[2, 0] = ct3 * st1 + ct1 * st2 * st3 C[2, 1] = -ct2 * st3 C[2, 2] = ct1 * ct3 - st1 * st2 * st3 return C
[docs]def euler2322C(q): """ euler2322C C = euler2322C(Q) returns the direction cosine matrix in terms of the 2-3-2 euler angles. Input Q must be a 3x1 vector of euler angles. """ st1 = math.sin(q[0]) ct1 = math.cos(q[0]) st2 = math.sin(q[1]) ct2 = math.cos(q[1]) st3 = math.sin(q[2]) ct3 = math.cos(q[2]) C = np.identity(3) C[0, 0] = ct1 * ct2 * ct3 - st1 * st3 C[0, 1] = ct3 * st2 C[0, 2] = -ct2 * ct3 * st1 - ct1 * st3 C[1, 0] = -ct1 * st2 C[1, 1] = ct2 C[1, 2] = st1 * st2 C[2, 0] = ct3 * st1 + ct1 * ct2 * st3 C[2, 1] = st2 * st3 C[2, 2] = ct1 * ct3 - ct2 * st1 * st3 return C
[docs]def euler3122C(q): """ euler3122C C = euler3122C(Q) returns the direction cosine matrix in terms of the 1-2-3 euler angles. Input Q must be a 3x1 vector of euler angles. """ st1 = math.sin(q[0]) ct1 = math.cos(q[0]) st2 = math.sin(q[1]) ct2 = math.cos(q[1]) st3 = math.sin(q[2]) ct3 = math.cos(q[2]) C = np.identity(3) C[0, 0] = ct1 * ct3 - st1 * st2 * st3 C[0, 1] = ct3 * st1 + ct1 * st2 * st3 C[0, 2] = -ct2 * st3 C[1, 0] = -ct2 * st1 C[1, 1] = ct1 * ct2 C[1, 2] = st2 C[2, 0] = ct3 * st1 * st2 + ct1 * st3 C[2, 1] = st1 * st3 - ct1 * ct3 * st2 C[2, 2] = ct2 * ct3 return C
[docs]def euler3132C(q): """ euler3132C C = euler3132C(Q) returns the direction cosine matrix in terms of the 3-1-3 euler angles. Input Q must be a 3x1 vector of euler angles. """ st1 = math.sin(q[0]) ct1 = math.cos(q[0]) st2 = math.sin(q[1]) ct2 = math.cos(q[1]) st3 = math.sin(q[2]) ct3 = math.cos(q[2]) C = np.identity(3) C[0, 0] = ct3 * ct1 - st3 * ct2 * st1 C[0, 1] = ct3 * st1 + st3 * ct2 * ct1 C[0, 2] = st3 * st2 C[1, 0] = -st3 * ct1 - ct3 * ct2 * st1 C[1, 1] = -st3 * st1 + ct3 * ct2 * ct1 C[1, 2] = ct3 * st2 C[2, 0] = st2 * st1 C[2, 1] = -st2 * ct1 C[2, 2] = ct2 return C
[docs]def euler3212C(q): """ euler3212C C = euler3212C(Q) returns the direction cosine matrix in terms of the 3-2-1 euler angles. Input Q must be a 3x1 vector of euler angles. """ st1 = math.sin(q[0]) ct1 = math.cos(q[0]) st2 = math.sin(q[1]) ct2 = math.cos(q[1]) st3 = math.sin(q[2]) ct3 = math.cos(q[2]) C = np.identity(3) C[0, 0] = ct2 * ct1 C[0, 1] = ct2 * st1 C[0, 2] = -st2 C[1, 0] = st3 * st2 * ct1 - ct3 * st1 C[1, 1] = st3 * st2 * st1 + ct3 * ct1 C[1, 2] = st3 * ct2 C[2, 0] = ct3 * st2 * ct1 + st3 * st1 C[2, 1] = ct3 * st2 * st1 - st3 * ct1 C[2, 2] = ct3 * ct2 return C
[docs]def euler3232C(q): """ euler3232C C = euler3232C(Q) returns the direction cosine matrix in terms of the 3-2-3 euler angles. Input Q must be a 3x1 vector of euler angles. """ st1 = math.sin(q[0]) ct1 = math.cos(q[0]) st2 = math.sin(q[1]) ct2 = math.cos(q[1]) st3 = math.sin(q[2]) ct3 = math.cos(q[2]) C = np.identity(3) C[0, 0] = ct1 * ct2 * ct3 - st1 * st3 C[0, 1] = ct2 * ct3 * st1 + ct1 * st3 C[0, 2] = -ct3 * st2 C[1, 0] = -ct3 * st1 - ct1 * ct2 * st3 C[1, 1] = ct1 * ct3 - ct2 * st1 * st3 C[1, 2] = st2 * st3 C[2, 0] = ct1 * st2 C[2, 1] = st1 * st2 C[2, 2] = ct2 return C
[docs]def euler2122EP(e): """ euler2122EP(E) Q = euler2122EP(E) translates the 212 euler angle vector E into the euler parameter vector Q. """ e1 = e[0] / 2 e2 = e[1] / 2 e3 = e[2] / 2 q0 = math.cos(e2) * math.cos(e1 + e3) q1 = math.sin(e2) * math.cos(-e1 + e3) q2 = math.cos(e2) * math.sin(e1 + e3) q3 = math.sin(e2) * math.sin(-e1 + e3) return np.array([q0, q1, q2, q3])
[docs]def euler2132EP(e): """ euler2132EP(E) Q = euler2132EP(E) translates the 213 euler angle vector E into the euler parameter vector Q. """ c1 = math.cos(e[0] / 2) s1 = math.sin(e[0] / 2) c2 = math.cos(e[1] / 2) s2 = math.sin(e[1] / 2) c3 = math.cos(e[2] / 2) s3 = math.sin(e[2] / 2) q0 = c1 * c2 * c3 + s1 * s2 * s3 q1 = c1 * s2 * c3 + s1 * c2 * s3 q2 = s1 * c2 * c3 - c1 * s2 * s3 q3 = c1 * c2 * s3 - s1 * s2 * c3 return np.array([q0, q1, q2, q3])
[docs]def euler2312EP(e): """ euler2312EP(E) Q = euler2312EP(E) translates the 231 euler angle vector E into the euler parameter vector Q. """ c1 = math.cos(e[0] / 2) s1 = math.sin(e[0] / 2) c2 = math.cos(e[1] / 2) s2 = math.sin(e[1] / 2) c3 = math.cos(e[2] / 2) s3 = math.sin(e[2] / 2) q0 = c1 * c2 * c3 - s1 * s2 * s3 q1 = c1 * c2 * s3 + s1 * s2 * c3 q2 = s1 * c2 * c3 + c1 * s2 * s3 q3 = c1 * s2 * c3 - s1 * c2 * s3 return np.array([q0, q1, q2, q3])
[docs]def euler2322EP(e): """ euler2322EP(E) Q = euler2322EP(E) translates the 232 euler angle vector E into the euler parameter vector Q. """ e1 = e[0] / 2 e2 = e[1] / 2 e3 = e[2] / 2 q0 = math.cos(e2) * math.cos(e1 + e3) q1 = math.sin(e2) * math.sin(e1 - e3) q2 = math.cos(e2) * math.sin(e1 + e3) q3 = math.sin(e2) * math.cos(e1 - e3) return np.array([q0, q1, q2, q3])
[docs]def euler3122EP(e): """ euler3122EP(E) Q = euler3122EP(E) translates the 312 euler angle vector E into the euler parameter vector Q. """ c1 = math.cos(e[0] / 2) s1 = math.sin(e[0] / 2) c2 = math.cos(e[1] / 2) s2 = math.sin(e[1] / 2) c3 = math.cos(e[2] / 2) s3 = math.sin(e[2] / 2) q0 = c1 * c2 * c3 - s1 * s2 * s3 q1 = c1 * s2 * c3 - s1 * c2 * s3 q2 = c1 * c2 * s3 + s1 * s2 * c3 q3 = s1 * c2 * c3 + c1 * s2 * s3 return np.array([q0, q1, q2, q3])
[docs]def euler3132EP(e): """ euler3132EP(E) Q = euler3132EP(E) translates the 313 euler angle vector E into the euler parameter vector Q. """ e1 = e[0] / 2 e2 = e[1] / 2 e3 = e[2] / 2 q0 = math.cos(e2) * math.cos(e1 + e3) q1 = math.sin(e2) * math.cos(e1 - e3) q2 = math.sin(e2) * math.sin(e1 - e3) q3 = math.cos(e2) * math.sin(e1 + e3) return np.array([q0, q1, q2, q3])
[docs]def euler3212EP(e): """ euler3212EP(E) Q = euler3212EP(E) translates the 321 euler angle vector E into the euler parameter vector Q. """ c1 = math.cos(e[0] / 2) s1 = math.sin(e[0] / 2) c2 = math.cos(e[1] / 2) s2 = math.sin(e[1] / 2) c3 = math.cos(e[2] / 2) s3 = math.sin(e[2] / 2) q0 = c1 * c2 * c3 + s1 * s2 * s3 q1 = c1 * c2 * s3 - s1 * s2 * c3 q2 = c1 * s2 * c3 + s1 * c2 * s3 q3 = s1 * c2 * c3 - c1 * s2 * s3 return np.array([q0, q1, q2, q3])
[docs]def euler3232EP(e): """ euler3232EP(E) Q = euler3232EP(E) translates the 323 euler angle vector E into the euler parameter vector Q. """ e1 = e[0] / 2 e2 = e[1] / 2 e3 = e[2] / 2 q0 = math.cos(e2) * math.cos(e1 + e3) q1 = math.sin(e2) * math.sin(-e1 + e3) q2 = math.sin(e2) * math.cos(-e1 + e3) q3 = math.cos(e2) * math.sin(e1 + e3) return np.array([q0, q1, q2, q3])
[docs]def euler2122Gibbs(e): """ euler2122Gibbs(E) Q = euler2122Gibbs(E) translates the (2-1-2) euler angle vector E into the gibbs vector Q. """ return EP2Gibbs(euler2122EP(e))
[docs]def euler2122MRP(e): """ euler2122MRP(E) Q = euler2122MRP(E) translates the (2-1-2) euler angle vector E into the MRP vector Q. """ return EP2MRP(euler2122EP(e))
[docs]def euler2122PRV(e): """ euler2122PRV(E) Q = euler2122PRV(E) translates the (2-1-2) euler angle vector E into the principal rotation vector Q. """ return EP2PRV(euler2122EP(e))
[docs]def euler2132Gibbs(e): """ euler2132Gibbs(E) Q = euler2132Gibbs(E) translates the (2-1-3) euler angle vector E into the gibbs vector Q. """ return EP2Gibbs(euler2132EP(e))
[docs]def euler2132MRP(e): """ euler2132MRP(E) Q = euler2132MRP(E) translates the (2-1-3) euler angle vector E into the MRP vector Q. """ return EP2MRP(euler2132EP(e))
[docs]def euler2132PRV(e): """ euler2132PRV(E) Q = euler2132PRV(E) translates the (2-1-3) euler angle vector E into the principal rotation vector Q. """ return EP2PRV(euler2132EP(e))
[docs]def euler2312Gibbs(e): """ euler2312Gibbs(E) Q = euler2312Gibbs(E) translates the (2-3-1) euler angle vector E into the gibbs vector Q. """ return EP2Gibbs(euler2312EP(e))
[docs]def euler2312MRP(e): """ euler2312MRP(E) Q = euler2312MRP(E) translates the (2-3-1) euler angle vector E into the MRP vector Q. """ return EP2MRP(euler2312EP(e))
[docs]def euler2312PRV(e): """ euler2312PRV(E) Q = euler2312PRV(E) translates the (2-3-1) euler angle vector E into the principal rotation vector Q. """ return EP2PRV(euler2312EP(e))
[docs]def euler2322Gibbs(e): """ euler2322Gibbs(E) Q = euler2322Gibbs(E) translates the (2-3-2) euler angle vector E into the gibbs vector Q. """ return EP2Gibbs(euler2322EP(e))
[docs]def euler2322MRP(e): """ euler2322MRP(E) Q = euler2322MRP(E) translates the (2-3-2) euler angle vector E into the MRP vector Q. """ return EP2MRP(euler2322EP(e))
[docs]def euler2322PRV(e): """ euler2322PRV(E) Q = euler2322PRV(E) translates the (2-3-2) euler angle vector E into the principal rotation vector Q. """ return EP2PRV(euler2322EP(e))
[docs]def euler3122Gibbs(e): """ euler3122Gibbs(E) Q = euler3122Gibbs(E) translates the (3-1-2) euler angle vector E into the gibbs vector Q. """ return EP2Gibbs(euler3122EP(e))
[docs]def euler3122MRP(e): """ euler3122MRP(E) Q = euler3122MRP(E) translates the (3-1-2) euler angle vector E into the MRP vector Q. """ return EP2MRP(euler3122EP(e))
[docs]def euler3122PRV(e): """ euler3122PRV(E) Q = euler3122PRV(E) translates the (3-1-2) euler angle vector E into the principal rotation vector Q. """ return EP2PRV(euler3122EP(e))
[docs]def euler3132Gibbs(e): """ euler3132Gibbs(E) Q = euler3132Gibbs(E) translates the (3-1-3) euler angle vector E into the gibbs vector Q. """ return EP2Gibbs(euler3132EP(e))
[docs]def euler3132MRP(e): """ euler3132MRP(E) Q = euler3132MRP(E) translates the (3-1-3) euler angle vector E into the MRP vector Q. """ return EP2MRP(euler3132EP(e))
[docs]def euler3132PRV(e): """ euler3132PRV(E) Q = euler3132PRV(E) translates the (3-1-3) euler angle vector E into the principal rotation vector Q. """ return EP2PRV(euler3132EP(e))
[docs]def euler3212Gibbs(e): """ euler3212Gibbs(E) Q = euler3212Gibbs(E) translates the (3-2-1) euler angle vector E into the gibbs vector Q. """ return EP2Gibbs(euler3212EP(e))
[docs]def euler3212MRP(e): """ euler3212MRP(E) Q = euler3212MRP(E) translates the (3-2-1) euler angle vector E into the MRP vector Q. """ return EP2MRP(euler3212EP(e))
[docs]def euler3212PRV(e): """ euler3212PRV(E) Q = euler3212PRV(E) translates the (3-2-1) euler angle vector E into the principal rotation vector Q. """ return EP2PRV(euler3212EP(e))
[docs]def euler3232Gibbs(e): """ euler3232Gibbs(E) Q = euler3232Gibbs(E) translates the (3-2-3) euler angle vector E into the gibbs vector Q. """ return EP2Gibbs(euler3232EP(e))
[docs]def euler3232MRP(e): """ euler3232MRP(E) Q = euler3232MRP(E) translates the (3-2-3) euler angle vector E into the MRP vector Q. """ return EP2MRP(euler3232EP(e))
[docs]def euler3232PRV(e): """ euler3232PRV(E) Q = euler3232PRV(E) translates the (3-2-3) euler angle vector Q1 into the principal rotation vector Q. """ return EP2PRV(euler3232EP(e))
def Mi(theta, i): c = np.cos(theta) s = np.sin(theta) case = i C = np.zeros((3, 3)) if case == 1: C[0][0] = 1. C[0][1] = 0. C[0][2] = 0. C[1][0] = 0. C[1][1] = c C[1][2] = s C[2][0] = 0. C[2][1] = -s C[2][2] = c elif case == 2: C[0][0] = c C[0][1] = 0. C[0][2] = -s C[1][0] = 0. C[1][1] = 1. C[1][2] = 0. C[2][0] = s C[2][1] = 0. C[2][2] = c elif case == 3: C[0][0] = c C[0][1] = s C[0][2] = 0. C[1][0] = -s C[1][1] = c C[1][2] = 0. C[2][0] = 0. C[2][1] = 0. C[2][2] = 1. else: print('Mi() error: incorrect axis', i, 'selected') return C def v3Tilde(vector): x1 = vector[0] x2 = vector[1] x3 = vector[2] xTilde = [[0, -x3, x2] ,[x3, 0, -x1] ,[-x2, x1, 0] ] return xTilde