def test_kin_eqs():
q0, q1, q2, q3 = dynamicsymbols('q0 q1 q2 q3')
q0d, q1d, q2d, q3d = dynamicsymbols('q0 q1 q2 q3', 1)
u1, u2, u3 = dynamicsymbols('u1 u2 u3')
kds = kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], 'quaternion')
assert kds == [-0.5 * q0 * u1 - 0.5 * q2 * u3 + 0.5 * q3 * u2 + q1d,
-0.5 * q0 * u2 + 0.5 * q1 * u3 - 0.5 * q3 * u1 + q2d,
-0.5 * q0 * u3 - 0.5 * q1 * u2 + 0.5 * q2 * u1 + q3d,
0.5 * q1 * u1 + 0.5 * q2 * u2 + 0.5 * q3 * u3 + q0d]
def orient(self, parent, rot_type, amounts, rot_order=''):
"""Defines the orientation of this frame relative to a parent frame.
Parameters
==========
parent : ReferenceFrame
The frame that this ReferenceFrame will have its orientation matrix
defined in relation to.
rot_type : str
The type of orientation matrix that is being created. Supported
types are 'Body', 'Space', 'Quaternion', and 'Axis'. See examples
for correct usage.
amounts : list OR value
The quantities that the orientation matrix will be defined by.
rot_order : str
If applicable, the order of a series of rotations.
Examples
========
>>> from sympy.physics.vector import ReferenceFrame, Vector
>>> from sympy import symbols
>>> q0, q1, q2, q3 = symbols('q0 q1 q2 q3')
>>> N = ReferenceFrame('N')
>>> B = ReferenceFrame('B')
Now we have a choice of how to implement the orientation. First is
Body. Body orientation takes this reference frame through three
successive simple rotations. Acceptable rotation orders are of length
3, expressed in XYZ or 123, and cannot have a rotation about about an
axis twice in a row.
>>> B.orient(N, 'Body', [q1, q2, q3], '123')
>>> B.orient(N, 'Body', [q1, q2, 0], 'ZXZ')
>>> B.orient(N, 'Body', [0, 0, 0], 'XYX')
Next is Space. Space is like Body, but the rotations are applied in the
opposite order.
>>> B.orient(N, 'Space', [q1, q2, q3], '312')
Next is Quaternion. This orients the new ReferenceFrame with
Quaternions, defined as a finite rotation about lambda, a unit vector,
by some amount theta.
This orientation is described by four parameters:
q0 = cos(theta/2)
q1 = lambda_x sin(theta/2)
q2 = lambda_y sin(theta/2)
q3 = lambda_z sin(theta/2)
Quaternion does not take in a rotation order.
>>> B.orient(N, 'Quaternion', [q0, q1, q2, q3])
Last is Axis. This is a rotation about an arbitrary, non-time-varying
axis by some angle. The axis is supplied as a Vector. This is how
simple rotations are defined.
>>> B.orient(N, 'Axis', [q1, N.x + 2 * N.y])
"""
from sympy.physics.vector.functions import dynamicsymbols
_check_frame(parent)
amounts = list(amounts)
for i, v in enumerate(amounts):
if not isinstance(v, Vector):
amounts[i] = sympify(v)
def _rot(axis, angle):
"""DCM for simple axis 1,2,or 3 rotations. """
if axis == 1:
return Matrix([[1, 0, 0],
[0, cos(angle), -sin(angle)],
[0, sin(angle), cos(angle)]])
elif axis == 2:
return Matrix([[cos(angle), 0, sin(angle)],
[0, 1, 0],
[-sin(angle), 0, cos(angle)]])
elif axis == 3:
return Matrix([[cos(angle), -sin(angle), 0],
[sin(angle), cos(angle), 0],
[0, 0, 1]])
approved_orders = ('123', '231', '312', '132', '213', '321', '121',
'131', '212', '232', '313', '323', '')
rot_order = str(
rot_order).upper() # Now we need to make sure XYZ = 123
rot_type = rot_type.upper()
rot_order = [i.replace('X', '1') for i in rot_order]
rot_order = [i.replace('Y', '2') for i in rot_order]
rot_order = [i.replace('Z', '3') for i in rot_order]
rot_order = ''.join(rot_order)
if not rot_order in approved_orders:
raise TypeError('The supplied order is not an approved type')
parent_orient = []
if rot_type == 'AXIS':
if not rot_order == '':
raise TypeError('Axis orientation takes no rotation order')
if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 2)):
raise TypeError('Amounts are a list or tuple of length 2')
theta = amounts[0]
axis = amounts[1]
axis = _check_vector(axis)
if not axis.dt(parent) == 0:
raise ValueError('Axis cannot be time-varying')
axis = axis.express(parent).normalize()
axis = axis.args[0][0]
parent_orient = ((eye(3) - axis * axis.T) * cos(theta) +
Matrix([[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]],
[-axis[1], axis[0], 0]]) * sin(theta) + axis * axis.T)
elif rot_type == 'QUATERNION':
if not rot_order == '':
raise TypeError(
'Quaternion orientation takes no rotation order')
if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 4)):
raise TypeError('Amounts are a list or tuple of length 4')
q0, q1, q2, q3 = amounts
parent_orient = (Matrix([[q0 ** 2 + q1 ** 2 - q2 ** 2 - q3 **
2, 2 * (q1 * q2 - q0 * q3), 2 * (q0 * q2 + q1 * q3)],
[2 * (q1 * q2 + q0 * q3), q0 ** 2 - q1 ** 2 + q2 ** 2 - q3 ** 2,
2 * (q2 * q3 - q0 * q1)], [2 * (q1 * q3 - q0 * q2), 2 * (q0 *
q1 + q2 * q3), q0 ** 2 - q1 ** 2 - q2 ** 2 + q3 ** 2]]))
elif rot_type == 'BODY':
if not (len(amounts) == 3 & len(rot_order) == 3):
raise TypeError('Body orientation takes 3 values & 3 orders')
a1 = int(rot_order[0])
a2 = int(rot_order[1])
a3 = int(rot_order[2])
parent_orient = (_rot(a1, amounts[0]) * _rot(a2, amounts[1])
* _rot(a3, amounts[2]))
elif rot_type == 'SPACE':
if not (len(amounts) == 3 & len(rot_order) == 3):
raise TypeError('Space orientation takes 3 values & 3 orders')
a1 = int(rot_order[0])
a2 = int(rot_order[1])
a3 = int(rot_order[2])
parent_orient = (_rot(a3, amounts[2]) * _rot(a2, amounts[1])
* _rot(a1, amounts[0]))
else:
raise NotImplementedError('That is not an implemented rotation')
#Reset the _dcm_cache of this frame, and remove it from the _dcm_caches
#of the frames it is linked to. Also remove it from the _dcm_dict of
#its parent
frames = self._dcm_cache.keys()
dcm_dict_del = []
dcm_cache_del = []
for frame in frames:
if frame in self._dcm_dict:
dcm_dict_del += [frame]
dcm_cache_del += [frame]
for frame in dcm_dict_del:
del frame._dcm_dict[self]
for frame in dcm_cache_del:
del frame._dcm_cache[self]
#Add the dcm relationship to _dcm_dict
self._dcm_dict = self._dlist[0] = {}
self._dcm_dict.update({parent: parent_orient.T})
parent._dcm_dict.update({self: parent_orient})
#Also update the dcm cache after resetting it
self._dcm_cache = {}
self._dcm_cache.update({parent: parent_orient.T})
parent._dcm_cache.update({self: parent_orient})
if rot_type == 'QUATERNION':
t = dynamicsymbols._t
q0, q1, q2, q3 = amounts
q0d = diff(q0, t)
q1d = diff(q1, t)
q2d = diff(q2, t)
q3d = diff(q3, t)
w1 = 2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1)
w2 = 2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2)
w3 = 2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3)
wvec = Vector([(Matrix([w1, w2, w3]), self)])
elif rot_type == 'AXIS':
thetad = (amounts[0]).diff(dynamicsymbols._t)
wvec = thetad * amounts[1].express(parent).normalize()
else:
try:
from sympy.polys.polyerrors import CoercionFailed
from sympy.physics.vector.functions import kinematic_equations
q1, q2, q3 = amounts
u1, u2, u3 = symbols('u1, u2, u3', cls=Dummy)
templist = kinematic_equations([u1, u2, u3], [q1, q2, q3],
rot_type, rot_order)
templist = [expand(i) for i in templist]
td = solve(templist, [u1, u2, u3])
u1 = expand(td[u1])
u2 = expand(td[u2])
u3 = expand(td[u3])
wvec = u1 * self.x + u2 * self.y + u3 * self.z
except (CoercionFailed, AssertionError):
wvec = self._w_diff_dcm(parent)
self._ang_vel_dict.update({parent: wvec})
parent._ang_vel_dict.update({self: -wvec})
self._var_dict = {}
def orient(self, parent, rot_type, amounts, rot_order=''):
"""Defines the orientation of this frame relative to a parent frame.
Parameters
==========
parent : ReferenceFrame
The frame that this ReferenceFrame will have its orientation matrix
defined in relation to.
rot_type : str
The type of orientation matrix that is being created. Supported
types are 'Body', 'Space', 'Quaternion', 'Axis', and 'DCM'.
See examples for correct usage.
amounts : list OR value
The quantities that the orientation matrix will be defined by.
In case of rot_type='DCM', value must be a
sympy.matrices.MatrixBase object (or subclasses of it).
rot_order : str or int
If applicable, the order of a series of rotations.
Examples
========
>>> from sympy.physics.vector import ReferenceFrame, Vector
>>> from sympy import symbols, eye, ImmutableMatrix
>>> q0, q1, q2, q3 = symbols('q0 q1 q2 q3')
>>> N = ReferenceFrame('N')
>>> B = ReferenceFrame('B')
Now we have a choice of how to implement the orientation. First is
Body. Body orientation takes this reference frame through three
successive simple rotations. Acceptable rotation orders are of length
3, expressed in XYZ or 123, and cannot have a rotation about about an
axis twice in a row.
>>> B.orient(N, 'Body', [q1, q2, q3], 123)
>>> B.orient(N, 'Body', [q1, q2, 0], 'ZXZ')
>>> B.orient(N, 'Body', [0, 0, 0], 'XYX')
Next is Space. Space is like Body, but the rotations are applied in the
opposite order.
>>> B.orient(N, 'Space', [q1, q2, q3], '312')
Next is Quaternion. This orients the new ReferenceFrame with
Quaternions, defined as a finite rotation about lambda, a unit vector,
by some amount theta.
This orientation is described by four parameters:
q0 = cos(theta/2)
q1 = lambda_x sin(theta/2)
q2 = lambda_y sin(theta/2)
q3 = lambda_z sin(theta/2)
Quaternion does not take in a rotation order.
>>> B.orient(N, 'Quaternion', [q0, q1, q2, q3])
Next is Axis. This is a rotation about an arbitrary, non-time-varying
axis by some angle. The axis is supplied as a Vector. This is how
simple rotations are defined.
>>> B.orient(N, 'Axis', [q1, N.x + 2 * N.y])
Last is DCM (Direction Cosine Matrix). This is a rotation matrix
given manually.
>>> B.orient(N, 'DCM', eye(3))
>>> B.orient(N, 'DCM', ImmutableMatrix([[0, 1, 0], [0, 0, -1], [-1, 0, 0]]))
"""
from sympy.physics.vector.functions import dynamicsymbols
_check_frame(parent)
# Allow passing a rotation matrix manually.
if rot_type == 'DCM':
# When rot_type == 'DCM', then amounts must be a Matrix type object
# (e.g. sympy.matrices.dense.MutableDenseMatrix).
if not isinstance(amounts, MatrixBase):
raise TypeError("Amounts must be a sympy Matrix type object.")
else:
amounts = list(amounts)
for i, v in enumerate(amounts):
if not isinstance(v, Vector):
amounts[i] = sympify(v)
def _rot(axis, angle):
"""DCM for simple axis 1,2,or 3 rotations. """
if axis == 1:
return Matrix([[1, 0, 0], [0, cos(angle), -sin(angle)],
[0, sin(angle), cos(angle)]])
elif axis == 2:
return Matrix([[cos(angle), 0, sin(angle)], [0, 1, 0],
[-sin(angle), 0, cos(angle)]])
elif axis == 3:
return Matrix([[cos(angle), -sin(angle), 0],
[sin(angle), cos(angle), 0], [0, 0, 1]])
approved_orders = ('123', '231', '312', '132', '213', '321', '121',
'131', '212', '232', '313', '323', '')
# make sure XYZ => 123 and rot_type is in upper case
rot_order = translate(str(rot_order), 'XYZxyz', '123123')
rot_type = rot_type.upper()
if not rot_order in approved_orders:
raise TypeError('The supplied order is not an approved type')
parent_orient = []
if rot_type == 'AXIS':
if not rot_order == '':
raise TypeError('Axis orientation takes no rotation order')
if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 2)):
raise TypeError('Amounts are a list or tuple of length 2')
theta = amounts[0]
axis = amounts[1]
axis = _check_vector(axis)
if not axis.dt(parent) == 0:
raise ValueError('Axis cannot be time-varying')
axis = axis.express(parent).normalize()
axis = axis.args[0][0]
parent_orient = (
(eye(3) - axis * axis.T) * cos(theta) +
Matrix([[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]],
[-axis[1], axis[0], 0]]) * sin(theta) + axis * axis.T)
elif rot_type == 'QUATERNION':
if not rot_order == '':
raise TypeError(
'Quaternion orientation takes no rotation order')
if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 4)):
raise TypeError('Amounts are a list or tuple of length 4')
q0, q1, q2, q3 = amounts
parent_orient = (Matrix([[
q0**2 + q1**2 - q2**2 - q3**2, 2 * (q1 * q2 - q0 * q3),
2 * (q0 * q2 + q1 * q3)
],
[
2 * (q1 * q2 + q0 * q3),
q0**2 - q1**2 + q2**2 - q3**2,
2 * (q2 * q3 - q0 * q1)
],
[
2 * (q1 * q3 - q0 * q2),
2 * (q0 * q1 + q2 * q3),
q0**2 - q1**2 - q2**2 + q3**2
]]))
elif rot_type == 'BODY':
if not (len(amounts) == 3 & len(rot_order) == 3):
raise TypeError('Body orientation takes 3 values & 3 orders')
a1 = int(rot_order[0])
a2 = int(rot_order[1])
a3 = int(rot_order[2])
parent_orient = (_rot(a1, amounts[0]) * _rot(a2, amounts[1]) *
_rot(a3, amounts[2]))
elif rot_type == 'SPACE':
if not (len(amounts) == 3 & len(rot_order) == 3):
raise TypeError('Space orientation takes 3 values & 3 orders')
a1 = int(rot_order[0])
a2 = int(rot_order[1])
a3 = int(rot_order[2])
parent_orient = (_rot(a3, amounts[2]) * _rot(a2, amounts[1]) *
_rot(a1, amounts[0]))
elif rot_type == 'DCM':
parent_orient = amounts
else:
raise NotImplementedError('That is not an implemented rotation')
#Reset the _dcm_cache of this frame, and remove it from the _dcm_caches
#of the frames it is linked to. Also remove it from the _dcm_dict of
#its parent
frames = self._dcm_cache.keys()
dcm_dict_del = []
dcm_cache_del = []
for frame in frames:
if frame in self._dcm_dict:
dcm_dict_del += [frame]
dcm_cache_del += [frame]
for frame in dcm_dict_del:
del frame._dcm_dict[self]
for frame in dcm_cache_del:
del frame._dcm_cache[self]
#Add the dcm relationship to _dcm_dict
self._dcm_dict = self._dlist[0] = {}
self._dcm_dict.update({parent: parent_orient.T})
parent._dcm_dict.update({self: parent_orient})
#Also update the dcm cache after resetting it
self._dcm_cache = {}
self._dcm_cache.update({parent: parent_orient.T})
parent._dcm_cache.update({self: parent_orient})
if rot_type == 'QUATERNION':
t = dynamicsymbols._t
q0, q1, q2, q3 = amounts
q0d = diff(q0, t)
q1d = diff(q1, t)
q2d = diff(q2, t)
q3d = diff(q3, t)
w1 = 2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1)
w2 = 2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2)
w3 = 2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3)
wvec = Vector([(Matrix([w1, w2, w3]), self)])
elif rot_type == 'AXIS':
thetad = (amounts[0]).diff(dynamicsymbols._t)
wvec = thetad * amounts[1].express(parent).normalize()
elif rot_type == 'DCM':
wvec = self._w_diff_dcm(parent)
else:
try:
from sympy.polys.polyerrors import CoercionFailed
from sympy.physics.vector.functions import kinematic_equations
q1, q2, q3 = amounts
u1, u2, u3 = symbols('u1, u2, u3', cls=Dummy)
templist = kinematic_equations([u1, u2, u3], [q1, q2, q3],
rot_type, rot_order)
templist = [expand(i) for i in templist]
td = solve(templist, [u1, u2, u3])
u1 = expand(td[u1])
u2 = expand(td[u2])
u3 = expand(td[u3])
wvec = u1 * self.x + u2 * self.y + u3 * self.z
except (CoercionFailed, AssertionError):
wvec = self._w_diff_dcm(parent)
self._ang_vel_dict.update({parent: wvec})
parent._ang_vel_dict.update({self: -wvec})
self._var_dict = {}
def test_kin_eqs():
q0, q1, q2, q3 = dynamicsymbols('q0 q1 q2 q3')
q0d, q1d, q2d, q3d = dynamicsymbols('q0 q1 q2 q3', 1)
u1, u2, u3 = dynamicsymbols('u1 u2 u3')
ke = kinematic_equations([u1, u2, u3], [q1, q2, q3], 'body', 313)
assert ke == kinematic_equations([u1, u2, u3], [q1, q2, q3], 'body', '313')
kds = kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], 'quaternion')
assert kds == [
-0.5 * q0 * u1 - 0.5 * q2 * u3 + 0.5 * q3 * u2 + q1d,
-0.5 * q0 * u2 + 0.5 * q1 * u3 - 0.5 * q3 * u1 + q2d,
-0.5 * q0 * u3 - 0.5 * q1 * u2 + 0.5 * q2 * u1 + q3d,
0.5 * q1 * u1 + 0.5 * q2 * u2 + 0.5 * q3 * u3 + q0d
]
raises(
ValueError,
lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2], 'quaternion'))
raises(
ValueError, lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3],
'quaternion', '123'))
raises(ValueError,
lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], 'foo'))
raises(TypeError,
lambda: kinematic_equations(u1, [q0, q1, q2, q3], 'quaternion'))
raises(TypeError,
lambda: kinematic_equations([u1], [q0, q1, q2, q3], 'quaternion'))
raises(TypeError,
lambda: kinematic_equations([u1, u2, u3], q0, 'quaternion'))
raises(ValueError,
lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], 'body'))
raises(
ValueError,
lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], 'space'))
raises(
ValueError,
lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2], 'body', '222'))
assert kinematic_equations([0, 0, 0], [q0, q1, q2],
'space') == [S.Zero, S.Zero, S.Zero]
def orient(self, parent, rot_type, amounts, rot_order=''):
"""Sets the orientation of this reference frame relative to another
(parent) reference frame.
Parameters
==========
parent : ReferenceFrame
Reference frame that this reference frame will be rotated relative
to.
rot_type : str
The method used to generate the direction cosine matrix. Supported
methods are:
- ``'Axis'``: simple rotations about a single common axis
- ``'DCM'``: for setting the direction cosine matrix directly
- ``'Body'``: three successive rotations about new intermediate
axes, also called "Euler and Tait-Bryan angles"
- ``'Space'``: three successive rotations about the parent
frames' unit vectors
- ``'Quaternion'``: rotations defined by four parameters which
result in a singularity free direction cosine matrix
amounts :
Expressions defining the rotation angles or direction cosine
matrix. These must match the ``rot_type``. See examples below for
details. The input types are:
- ``'Axis'``: 2-tuple (expr/sym/func, Vector)
- ``'DCM'``: Matrix, shape(3,3)
- ``'Body'``: 3-tuple of expressions, symbols, or functions
- ``'Space'``: 3-tuple of expressions, symbols, or functions
- ``'Quaternion'``: 4-tuple of expressions, symbols, or
functions
rot_order : str or int, optional
If applicable, the order of the successive of rotations. The string
``'123'`` and integer ``123`` are equivalent, for example. Required
for ``'Body'`` and ``'Space'``.
Examples
========
Setup variables for the examples:
>>> from sympy import symbols
>>> from sympy.physics.vector import ReferenceFrame
>>> q0, q1, q2, q3 = symbols('q0 q1 q2 q3')
>>> N = ReferenceFrame('N')
>>> B = ReferenceFrame('B')
>>> B1 = ReferenceFrame('B')
>>> B2 = ReferenceFrame('B2')
Axis
----
``rot_type='Axis'`` creates a direction cosine matrix defined by a
simple rotation about a single axis fixed in both reference frames.
This is a rotation about an arbitrary, non-time-varying
axis by some angle. The axis is supplied as a Vector. This is how
simple rotations are defined.
>>> B.orient(N, 'Axis', (q1, N.x))
The ``orient()`` method generates a direction cosine matrix and its
transpose which defines the orientation of B relative to N and vice
versa. Once orient is called, ``dcm()`` outputs the appropriate
direction cosine matrix.
>>> B.dcm(N)
Matrix([
[1, 0, 0],
[0, cos(q1), sin(q1)],
[0, -sin(q1), cos(q1)]])
The following two lines show how the sense of the rotation can be
defined. Both lines produce the same result.
>>> B.orient(N, 'Axis', (q1, -N.x))
>>> B.orient(N, 'Axis', (-q1, N.x))
The axis does not have to be defined by a unit vector, it can be any
vector in the parent frame.
>>> B.orient(N, 'Axis', (q1, N.x + 2 * N.y))
DCM
---
The direction cosine matrix can be set directly. The orientation of a
frame A can be set to be the same as the frame B above like so:
>>> B.orient(N, 'Axis', (q1, N.x))
>>> A = ReferenceFrame('A')
>>> A.orient(N, 'DCM', N.dcm(B))
>>> A.dcm(N)
Matrix([
[1, 0, 0],
[0, cos(q1), sin(q1)],
[0, -sin(q1), cos(q1)]])
**Note carefully that** ``N.dcm(B)`` **was passed into** ``orient()``
**for** ``A.dcm(N)`` **to match** ``B.dcm(N)``.
Body
----
``rot_type='Body'`` rotates this reference frame relative to the
provided reference frame by rotating through three successive simple
rotations. Each subsequent axis of rotation is about the "body fixed"
unit vectors of the new intermediate reference frame. This type of
rotation is also referred to rotating through the `Euler and Tait-Bryan
Angles <https://en.wikipedia.org/wiki/Euler_angles>`_.
For example, the classic Euler Angle rotation can be done by:
>>> B.orient(N, 'Body', (q1, q2, q3), 'XYX')
>>> B.dcm(N)
Matrix([
[ cos(q2), sin(q1)*sin(q2), -sin(q2)*cos(q1)],
[sin(q2)*sin(q3), -sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3), sin(q1)*cos(q3) + sin(q3)*cos(q1)*cos(q2)],
[sin(q2)*cos(q3), -sin(q1)*cos(q2)*cos(q3) - sin(q3)*cos(q1), -sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3)]])
This rotates B relative to N through ``q1`` about ``N.x``, then rotates
B again through q2 about B.y, and finally through q3 about B.x. It is
equivalent to:
>>> B1.orient(N, 'Axis', (q1, N.x))
>>> B2.orient(B1, 'Axis', (q2, B1.y))
>>> B.orient(B2, 'Axis', (q3, B2.x))
>>> B.dcm(N)
Matrix([
[ cos(q2), sin(q1)*sin(q2), -sin(q2)*cos(q1)],
[sin(q2)*sin(q3), -sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3), sin(q1)*cos(q3) + sin(q3)*cos(q1)*cos(q2)],
[sin(q2)*cos(q3), -sin(q1)*cos(q2)*cos(q3) - sin(q3)*cos(q1), -sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3)]])
Acceptable rotation orders are of length 3, expressed in as a string
``'XYZ'`` or ``'123'`` or integer ``123``. Rotations about an axis
twice in a row are prohibited.
>>> B.orient(N, 'Body', (q1, q2, 0), 'ZXZ')
>>> B.orient(N, 'Body', (q1, q2, 0), '121')
>>> B.orient(N, 'Body', (q1, q2, q3), 123)
Space
-----
``rot_type='Space'`` also rotates the reference frame in three
successive simple rotations but the axes of rotation are the
"Space-fixed" axes. For example:
>>> B.orient(N, 'Space', (q1, q2, q3), '312')
>>> B.dcm(N)
Matrix([
[ sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3), sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1)],
[-sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1), cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3)],
[ sin(q3)*cos(q2), -sin(q2), cos(q2)*cos(q3)]])
is equivalent to:
>>> B1.orient(N, 'Axis', (q1, N.z))
>>> B2.orient(B1, 'Axis', (q2, N.x))
>>> B.orient(B2, 'Axis', (q3, N.y))
>>> B.dcm(N).simplify() # doctest: +SKIP
Matrix([
[ sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3), sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1)],
[-sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1), cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3)],
[ sin(q3)*cos(q2), -sin(q2), cos(q2)*cos(q3)]])
It is worth noting that space-fixed and body-fixed rotations are
related by the order of the rotations, i.e. the reverse order of body
fixed will give space fixed and vice versa.
>>> B.orient(N, 'Space', (q1, q2, q3), '231')
>>> B.dcm(N)
Matrix([
[cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3), -sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1)],
[ -sin(q2), cos(q2)*cos(q3), sin(q3)*cos(q2)],
[sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1), sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3)]])
>>> B.orient(N, 'Body', (q3, q2, q1), '132')
>>> B.dcm(N)
Matrix([
[cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3), -sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1)],
[ -sin(q2), cos(q2)*cos(q3), sin(q3)*cos(q2)],
[sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1), sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3)]])
Quaternion
----------
``rot_type='Quaternion'`` orients the reference frame using
quaternions. Quaternion rotation is defined as a finite rotation about
lambda, a unit vector, by an amount theta. This orientation is
described by four parameters:
- ``q0 = cos(theta/2)``
- ``q1 = lambda_x sin(theta/2)``
- ``q2 = lambda_y sin(theta/2)``
- ``q3 = lambda_z sin(theta/2)``
This type does not need a ``rot_order``.
>>> B.orient(N, 'Quaternion', (q0, q1, q2, q3))
>>> B.dcm(N)
Matrix([
[q0**2 + q1**2 - q2**2 - q3**2, 2*q0*q3 + 2*q1*q2, -2*q0*q2 + 2*q1*q3],
[ -2*q0*q3 + 2*q1*q2, q0**2 - q1**2 + q2**2 - q3**2, 2*q0*q1 + 2*q2*q3],
[ 2*q0*q2 + 2*q1*q3, -2*q0*q1 + 2*q2*q3, q0**2 - q1**2 - q2**2 + q3**2]])
"""
from sympy.physics.vector.functions import dynamicsymbols
_check_frame(parent)
# Allow passing a rotation matrix manually.
if rot_type == 'DCM':
# When rot_type == 'DCM', then amounts must be a Matrix type object
# (e.g. sympy.matrices.dense.MutableDenseMatrix).
if not isinstance(amounts, MatrixBase):
raise TypeError("Amounts must be a sympy Matrix type object.")
else:
amounts = list(amounts)
for i, v in enumerate(amounts):
if not isinstance(v, Vector):
amounts[i] = sympify(v)
def _rot(axis, angle):
"""DCM for simple axis 1,2,or 3 rotations. """
if axis == 1:
return Matrix([[1, 0, 0], [0, cos(angle), -sin(angle)],
[0, sin(angle), cos(angle)]])
elif axis == 2:
return Matrix([[cos(angle), 0, sin(angle)], [0, 1, 0],
[-sin(angle), 0, cos(angle)]])
elif axis == 3:
return Matrix([[cos(angle), -sin(angle), 0],
[sin(angle), cos(angle), 0], [0, 0, 1]])
approved_orders = ('123', '231', '312', '132', '213', '321', '121',
'131', '212', '232', '313', '323', '')
# make sure XYZ => 123 and rot_type is in upper case
rot_order = translate(str(rot_order), 'XYZxyz', '123123')
rot_type = rot_type.upper()
if rot_order not in approved_orders:
raise TypeError('The supplied order is not an approved type')
parent_orient = []
if rot_type == 'AXIS':
if not rot_order == '':
raise TypeError('Axis orientation takes no rotation order')
if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 2)):
raise TypeError('Amounts are a list or tuple of length 2')
theta = amounts[0]
axis = amounts[1]
axis = _check_vector(axis)
if not axis.dt(parent) == 0:
raise ValueError('Axis cannot be time-varying')
axis = axis.express(parent).normalize()
axis = axis.args[0][0]
parent_orient = (
(eye(3) - axis * axis.T) * cos(theta) +
Matrix([[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]],
[-axis[1], axis[0], 0]]) * sin(theta) + axis * axis.T)
elif rot_type == 'QUATERNION':
if not rot_order == '':
raise TypeError(
'Quaternion orientation takes no rotation order')
if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 4)):
raise TypeError('Amounts are a list or tuple of length 4')
q0, q1, q2, q3 = amounts
parent_orient = (Matrix([[
q0**2 + q1**2 - q2**2 - q3**2, 2 * (q1 * q2 - q0 * q3),
2 * (q0 * q2 + q1 * q3)
],
[
2 * (q1 * q2 + q0 * q3),
q0**2 - q1**2 + q2**2 - q3**2,
2 * (q2 * q3 - q0 * q1)
],
[
2 * (q1 * q3 - q0 * q2),
2 * (q0 * q1 + q2 * q3),
q0**2 - q1**2 - q2**2 + q3**2
]]))
elif rot_type == 'BODY':
if not (len(amounts) == 3 & len(rot_order) == 3):
raise TypeError('Body orientation takes 3 values & 3 orders')
a1 = int(rot_order[0])
a2 = int(rot_order[1])
a3 = int(rot_order[2])
parent_orient = (_rot(a1, amounts[0]) * _rot(a2, amounts[1]) *
_rot(a3, amounts[2]))
elif rot_type == 'SPACE':
if not (len(amounts) == 3 & len(rot_order) == 3):
raise TypeError('Space orientation takes 3 values & 3 orders')
a1 = int(rot_order[0])
a2 = int(rot_order[1])
a3 = int(rot_order[2])
parent_orient = (_rot(a3, amounts[2]) * _rot(a2, amounts[1]) *
_rot(a1, amounts[0]))
elif rot_type == 'DCM':
parent_orient = amounts
else:
raise NotImplementedError('That is not an implemented rotation')
# Reset the _dcm_cache of this frame, and remove it from the
# _dcm_caches of the frames it is linked to. Also remove it from the
# _dcm_dict of its parent
frames = self._dcm_cache.keys()
dcm_dict_del = []
dcm_cache_del = []
for frame in frames:
if frame in self._dcm_dict:
dcm_dict_del += [frame]
dcm_cache_del += [frame]
for frame in dcm_dict_del:
del frame._dcm_dict[self]
for frame in dcm_cache_del:
del frame._dcm_cache[self]
# Add the dcm relationship to _dcm_dict
self._dcm_dict = self._dlist[0] = {}
self._dcm_dict.update({parent: parent_orient.T})
parent._dcm_dict.update({self: parent_orient})
# Also update the dcm cache after resetting it
self._dcm_cache = {}
self._dcm_cache.update({parent: parent_orient.T})
parent._dcm_cache.update({self: parent_orient})
if rot_type == 'QUATERNION':
t = dynamicsymbols._t
q0, q1, q2, q3 = amounts
q0d = diff(q0, t)
q1d = diff(q1, t)
q2d = diff(q2, t)
q3d = diff(q3, t)
w1 = 2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1)
w2 = 2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2)
w3 = 2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3)
wvec = Vector([(Matrix([w1, w2, w3]), self)])
elif rot_type == 'AXIS':
thetad = (amounts[0]).diff(dynamicsymbols._t)
wvec = thetad * amounts[1].express(parent).normalize()
elif rot_type == 'DCM':
wvec = self._w_diff_dcm(parent)
else:
try:
from sympy.polys.polyerrors import CoercionFailed
from sympy.physics.vector.functions import kinematic_equations
q1, q2, q3 = amounts
u1, u2, u3 = symbols('u1, u2, u3', cls=Dummy)
templist = kinematic_equations([u1, u2, u3], [q1, q2, q3],
rot_type, rot_order)
templist = [expand(i) for i in templist]
td = solve(templist, [u1, u2, u3])
u1 = expand(td[u1])
u2 = expand(td[u2])
u3 = expand(td[u3])
wvec = u1 * self.x + u2 * self.y + u3 * self.z
except (CoercionFailed, AssertionError):
wvec = self._w_diff_dcm(parent)
self._ang_vel_dict.update({parent: wvec})
parent._ang_vel_dict.update({self: -wvec})
self._var_dict = {}
def test_kin_eqs():
q0, q1, q2, q3 = dynamicsymbols('q0 q1 q2 q3')
q0d, q1d, q2d, q3d = dynamicsymbols('q0 q1 q2 q3', 1)
u1, u2, u3 = dynamicsymbols('u1 u2 u3')
ke = kinematic_equations([u1,u2,u3], [q1,q2,q3], 'body', 313)
assert ke == kinematic_equations([u1,u2,u3], [q1,q2,q3], 'body', '313')
kds = kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], 'quaternion')
assert kds == [-0.5 * q0 * u1 - 0.5 * q2 * u3 + 0.5 * q3 * u2 + q1d,
-0.5 * q0 * u2 + 0.5 * q1 * u3 - 0.5 * q3 * u1 + q2d,
-0.5 * q0 * u3 - 0.5 * q1 * u2 + 0.5 * q2 * u1 + q3d,
0.5 * q1 * u1 + 0.5 * q2 * u2 + 0.5 * q3 * u3 + q0d]
raises(ValueError, lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2], 'quaternion'))
raises(ValueError, lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], 'quaternion', '123'))
raises(ValueError, lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], 'foo'))
raises(TypeError, lambda: kinematic_equations(u1, [q0, q1, q2, q3], 'quaternion'))
raises(TypeError, lambda: kinematic_equations([u1], [q0, q1, q2, q3], 'quaternion'))
raises(TypeError, lambda: kinematic_equations([u1, u2, u3], q0, 'quaternion'))
raises(ValueError, lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], 'body'))
raises(ValueError, lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], 'space'))
raises(ValueError, lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2], 'body', '222'))
assert kinematic_equations([0, 0, 0], [q0, q1, q2], 'space') == [S.Zero, S.Zero, S.Zero]
def test_kin_eqs():
q0, q1, q2, q3 = dynamicsymbols("q0 q1 q2 q3")
q0d, q1d, q2d, q3d = dynamicsymbols("q0 q1 q2 q3", 1)
u1, u2, u3 = dynamicsymbols("u1 u2 u3")
ke = kinematic_equations([u1, u2, u3], [q1, q2, q3], "body", 313)
assert ke == kinematic_equations([u1, u2, u3], [q1, q2, q3], "body", "313")
kds = kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], "quaternion")
assert kds == [
-0.5 * q0 * u1 - 0.5 * q2 * u3 + 0.5 * q3 * u2 + q1d,
-0.5 * q0 * u2 + 0.5 * q1 * u3 - 0.5 * q3 * u1 + q2d,
-0.5 * q0 * u3 - 0.5 * q1 * u2 + 0.5 * q2 * u1 + q3d,
0.5 * q1 * u1 + 0.5 * q2 * u2 + 0.5 * q3 * u3 + q0d,
]
raises(
ValueError,
lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2], "quaternion"),
)
raises(
ValueError,
lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3],
"quaternion", "123"),
)
raises(ValueError,
lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], "foo"))
raises(TypeError,
lambda: kinematic_equations(u1, [q0, q1, q2, q3], "quaternion"))
raises(TypeError,
lambda: kinematic_equations([u1], [q0, q1, q2, q3], "quaternion"))
raises(TypeError,
lambda: kinematic_equations([u1, u2, u3], q0, "quaternion"))
raises(ValueError,
lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], "body"))
raises(
ValueError,
lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], "space"))
raises(
ValueError,
lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2], "body", "222"),
)
assert kinematic_equations([0, 0, 0], [q0, q1, q2], "space") == [
S.Zero,
S.Zero,
S.Zero,
]
def orient_space_fixed(self, parent, angles, rotation_order):
"""Rotates this reference frame relative to the parent reference frame
by right hand rotating through three successive space fixed simple axis
rotations. Each subsequent axis of rotation is about the "space fixed"
unit vectors of the parent reference frame.
Parameters
==========
parent : ReferenceFrame
Reference frame that this reference frame will be rotated relative
to.
angles : 3-tuple of sympifiable
Three angles in radians used for the successive rotations.
rotation_order : 3 character string or 3 digit integer
Order of the rotations about the parent reference frame's unit
vectors. The order can be specified by the strings ``'XZX'``,
``'131'``, or the integer ``131``. There are 12 unique valid
rotation orders.
Examples
========
Setup variables for the examples:
>>> from sympy import symbols
>>> from sympy.physics.vector import ReferenceFrame
>>> q1, q2, q3 = symbols('q1, q2, q3')
>>> N = ReferenceFrame('N')
>>> B = ReferenceFrame('B')
>>> B1 = ReferenceFrame('B1')
>>> B2 = ReferenceFrame('B2')
>>> B.orient_space_fixed(N, (q1, q2, q3), '312')
>>> B.dcm(N)
Matrix([
[ sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3), sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1)],
[-sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1), cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3)],
[ sin(q3)*cos(q2), -sin(q2), cos(q2)*cos(q3)]])
is equivalent to:
>>> B1.orient_axis(N, N.z, q1)
>>> B2.orient_axis(B1, N.x, q2)
>>> B.orient_axis(B2, N.y, q3)
>>> B.dcm(N).simplify()
Matrix([
[ sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3), sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1)],
[-sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1), cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3)],
[ sin(q3)*cos(q2), -sin(q2), cos(q2)*cos(q3)]])
It is worth noting that space-fixed and body-fixed rotations are
related by the order of the rotations, i.e. the reverse order of body
fixed will give space fixed and vice versa.
>>> B.orient_space_fixed(N, (q1, q2, q3), '231')
>>> B.dcm(N)
Matrix([
[cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3), -sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1)],
[ -sin(q2), cos(q2)*cos(q3), sin(q3)*cos(q2)],
[sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1), sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3)]])
>>> B.orient_body_fixed(N, (q3, q2, q1), '132')
>>> B.dcm(N)
Matrix([
[cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3), -sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1)],
[ -sin(q2), cos(q2)*cos(q3), sin(q3)*cos(q2)],
[sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1), sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3)]])
"""
_check_frame(parent)
amounts = list(angles)
for i, v in enumerate(amounts):
if not isinstance(v, Vector):
amounts[i] = sympify(v)
approved_orders = ('123', '231', '312', '132', '213', '321', '121',
'131', '212', '232', '313', '323', '')
# make sure XYZ => 123
rot_order = translate(str(rotation_order), 'XYZxyz', '123123')
if rot_order not in approved_orders:
raise TypeError('The supplied order is not an approved type')
parent_orient_space = []
if not (len(amounts) == 3 & len(rot_order) == 3):
raise TypeError('Space orientation takes 3 values & 3 orders')
a1 = int(rot_order[0])
a2 = int(rot_order[1])
a3 = int(rot_order[2])
parent_orient_space = (self._rot(a3, amounts[2]) *
self._rot(a2, amounts[1]) *
self._rot(a1, amounts[0]))
self._dcm(parent, parent_orient_space)
try:
from sympy.polys.polyerrors import CoercionFailed
from sympy.physics.vector.functions import kinematic_equations
q1, q2, q3 = amounts
u1, u2, u3 = symbols('u1, u2, u3', cls=Dummy)
templist = kinematic_equations([u1, u2, u3], [q1, q2, q3],
'space', rot_order)
templist = [expand(i) for i in templist]
td = solve(templist, [u1, u2, u3])
u1 = expand(td[u1])
u2 = expand(td[u2])
u3 = expand(td[u3])
wvec = u1 * self.x + u2 * self.y + u3 * self.z
except (CoercionFailed, AssertionError):
wvec = self._w_diff_dcm(parent)
self._ang_vel_dict.update({parent: wvec})
parent._ang_vel_dict.update({self: -wvec})
self._var_dict = {}
def orient_body_fixed(self, parent, angles, rotation_order):
"""Rotates this reference frame relative to the parent reference frame
by right hand rotating through three successive body fixed simple axis
rotations. Each subsequent axis of rotation is about the "body fixed"
unit vectors of a new intermediate reference frame. This type of
rotation is also referred to rotating through the `Euler and Tait-Bryan
Angles`_.
.. _Euler and Tait-Bryan Angles: https://en.wikipedia.org/wiki/Euler_angles
Parameters
==========
parent : ReferenceFrame
Reference frame that this reference frame will be rotated relative
to.
angles : 3-tuple of sympifiable
Three angles in radians used for the successive rotations.
rotation_order : 3 character string or 3 digit integer
Order of the rotations about each intermediate reference frames'
unit vectors. The Euler rotation about the X, Z', X'' axes can be
specified by the strings ``'XZX'``, ``'131'``, or the integer
``131``. There are 12 unique valid rotation orders (6 Euler and 6
Tait-Bryan): zxz, xyx, yzy, zyz, xzx, yxy, xyz, yzx, zxy, xzy, zyx,
and yxz.
Examples
========
Setup variables for the examples:
>>> from sympy import symbols
>>> from sympy.physics.vector import ReferenceFrame
>>> q1, q2, q3 = symbols('q1, q2, q3')
>>> N = ReferenceFrame('N')
>>> B = ReferenceFrame('B')
>>> B1 = ReferenceFrame('B1')
>>> B2 = ReferenceFrame('B2')
For example, a classic Euler Angle rotation can be done by:
>>> B.orient_body_fixed(N, (q1, q2, q3), 'XYX')
>>> B.dcm(N)
Matrix([
[ cos(q2), sin(q1)*sin(q2), -sin(q2)*cos(q1)],
[sin(q2)*sin(q3), -sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3), sin(q1)*cos(q3) + sin(q3)*cos(q1)*cos(q2)],
[sin(q2)*cos(q3), -sin(q1)*cos(q2)*cos(q3) - sin(q3)*cos(q1), -sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3)]])
This rotates reference frame B relative to reference frame N through
``q1`` about ``N.x``, then rotates B again through ``q2`` about
``B.y``, and finally through ``q3`` about ``B.x``. It is equivalent to
three successive ``orient_axis()`` calls:
>>> B1.orient_axis(N, N.x, q1)
>>> B2.orient_axis(B1, B1.y, q2)
>>> B.orient_axis(B2, B2.x, q3)
>>> B.dcm(N)
Matrix([
[ cos(q2), sin(q1)*sin(q2), -sin(q2)*cos(q1)],
[sin(q2)*sin(q3), -sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3), sin(q1)*cos(q3) + sin(q3)*cos(q1)*cos(q2)],
[sin(q2)*cos(q3), -sin(q1)*cos(q2)*cos(q3) - sin(q3)*cos(q1), -sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3)]])
Acceptable rotation orders are of length 3, expressed in as a string
``'XYZ'`` or ``'123'`` or integer ``123``. Rotations about an axis
twice in a row are prohibited.
>>> B.orient_body_fixed(N, (q1, q2, 0), 'ZXZ')
>>> B.orient_body_fixed(N, (q1, q2, 0), '121')
>>> B.orient_body_fixed(N, (q1, q2, q3), 123)
"""
_check_frame(parent)
amounts = list(angles)
for i, v in enumerate(amounts):
if not isinstance(v, Vector):
amounts[i] = sympify(v)
approved_orders = ('123', '231', '312', '132', '213', '321', '121',
'131', '212', '232', '313', '323', '')
# make sure XYZ => 123
rot_order = translate(str(rotation_order), 'XYZxyz', '123123')
if rot_order not in approved_orders:
raise TypeError('The rotation order is not a valid order.')
parent_orient_body = []
if not (len(amounts) == 3 & len(rot_order) == 3):
raise TypeError('Body orientation takes 3 values & 3 orders')
a1 = int(rot_order[0])
a2 = int(rot_order[1])
a3 = int(rot_order[2])
parent_orient_body = (self._rot(a1, amounts[0]) *
self._rot(a2, amounts[1]) *
self._rot(a3, amounts[2]))
self._dcm(parent, parent_orient_body)
try:
from sympy.polys.polyerrors import CoercionFailed
from sympy.physics.vector.functions import kinematic_equations
q1, q2, q3 = amounts
u1, u2, u3 = symbols('u1, u2, u3', cls=Dummy)
templist = kinematic_equations([u1, u2, u3], [q1, q2, q3],
'body', rot_order)
templist = [expand(i) for i in templist]
td = solve(templist, [u1, u2, u3])
u1 = expand(td[u1])
u2 = expand(td[u2])
u3 = expand(td[u3])
wvec = u1 * self.x + u2 * self.y + u3 * self.z
except (CoercionFailed, AssertionError):
wvec = self._w_diff_dcm(parent)
self._ang_vel_dict.update({parent: wvec})
parent._ang_vel_dict.update({self: -wvec})
self._var_dict = {}
