399 lines
12 KiB
Python
399 lines
12 KiB
Python
|
from sympy.core.basic import Basic
|
||
|
|
from sympy.core.sympify import sympify
|
||
|
|
from sympy.functions.elementary.trigonometric import (cos, sin)
|
||
|
|
from sympy.matrices.dense import (eye, rot_axis1, rot_axis2, rot_axis3)
|
||
|
|
from sympy.matrices.immutable import ImmutableDenseMatrix as Matrix
|
||
|
|
from sympy.core.cache import cacheit
|
||
|
|
from sympy.core.symbol import Str
|
||
|
|
import sympy.vector
|
||
|
|
|
||
|
|
|
||
|
|
class Orienter(Basic):
|
||
|
|
"""
|
||
|
|
Super-class for all orienter classes.
|
||
|
|
"""
|
||
|
|
|
||
|
|
def rotation_matrix(self):
|
||
|
|
"""
|
||
|
|
The rotation matrix corresponding to this orienter
|
||
|
|
instance.
|
||
|
|
"""
|
||
|
|
return self._parent_orient
|
||
|
|
|
||
|
|
|
||
|
|
class AxisOrienter(Orienter):
|
||
|
|
"""
|
||
|
|
Class to denote an axis orienter.
|
||
|
|
"""
|
||
|
|
|
||
|
|
def __new__(cls, angle, axis):
|
||
|
|
if not isinstance(axis, sympy.vector.Vector):
|
||
|
|
raise TypeError("axis should be a Vector")
|
||
|
|
angle = sympify(angle)
|
||
|
|
|
||
|
|
obj = super().__new__(cls, angle, axis)
|
||
|
|
obj._angle = angle
|
||
|
|
obj._axis = axis
|
||
|
|
|
||
|
|
return obj
|
||
|
|
|
||
|
|
def __init__(self, angle, axis):
|
||
|
|
"""
|
||
|
|
Axis rotation is a rotation about an arbitrary axis by
|
||
|
|
some angle. The angle is supplied as a SymPy expr scalar, and
|
||
|
|
the axis is supplied as a Vector.
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
==========
|
||
|
|
|
||
|
|
angle : Expr
|
||
|
|
The angle by which the new system is to be rotated
|
||
|
|
|
||
|
|
axis : Vector
|
||
|
|
The axis around which the rotation has to be performed
|
||
|
|
|
||
|
|
Examples
|
||
|
|
========
|
||
|
|
|
||
|
|
>>> from sympy.vector import CoordSys3D
|
||
|
|
>>> from sympy import symbols
|
||
|
|
>>> q1 = symbols('q1')
|
||
|
|
>>> N = CoordSys3D('N')
|
||
|
|
>>> from sympy.vector import AxisOrienter
|
||
|
|
>>> orienter = AxisOrienter(q1, N.i + 2 * N.j)
|
||
|
|
>>> B = N.orient_new('B', (orienter, ))
|
||
|
|
|
||
|
|
"""
|
||
|
|
# Dummy initializer for docstrings
|
||
|
|
pass
|
||
|
|
|
||
|
|
@cacheit
|
||
|
|
def rotation_matrix(self, system):
|
||
|
|
"""
|
||
|
|
The rotation matrix corresponding to this orienter
|
||
|
|
instance.
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
==========
|
||
|
|
|
||
|
|
system : CoordSys3D
|
||
|
|
The coordinate system wrt which the rotation matrix
|
||
|
|
is to be computed
|
||
|
|
"""
|
||
|
|
|
||
|
|
axis = sympy.vector.express(self.axis, system).normalize()
|
||
|
|
axis = axis.to_matrix(system)
|
||
|
|
theta = self.angle
|
||
|
|
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)
|
||
|
|
parent_orient = parent_orient.T
|
||
|
|
return parent_orient
|
||
|
|
|
||
|
|
@property
|
||
|
|
def angle(self):
|
||
|
|
return self._angle
|
||
|
|
|
||
|
|
@property
|
||
|
|
def axis(self):
|
||
|
|
return self._axis
|
||
|
|
|
||
|
|
|
||
|
|
class ThreeAngleOrienter(Orienter):
|
||
|
|
"""
|
||
|
|
Super-class for Body and Space orienters.
|
||
|
|
"""
|
||
|
|
|
||
|
|
def __new__(cls, angle1, angle2, angle3, rot_order):
|
||
|
|
if isinstance(rot_order, Str):
|
||
|
|
rot_order = rot_order.name
|
||
|
|
|
||
|
|
approved_orders = ('123', '231', '312', '132', '213',
|
||
|
|
'321', '121', '131', '212', '232',
|
||
|
|
'313', '323', '')
|
||
|
|
original_rot_order = rot_order
|
||
|
|
rot_order = str(rot_order).upper()
|
||
|
|
if not (len(rot_order) == 3):
|
||
|
|
raise TypeError('rot_order should be a str of length 3')
|
||
|
|
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 rot_order not in approved_orders:
|
||
|
|
raise TypeError('Invalid rot_type parameter')
|
||
|
|
a1 = int(rot_order[0])
|
||
|
|
a2 = int(rot_order[1])
|
||
|
|
a3 = int(rot_order[2])
|
||
|
|
angle1 = sympify(angle1)
|
||
|
|
angle2 = sympify(angle2)
|
||
|
|
angle3 = sympify(angle3)
|
||
|
|
if cls._in_order:
|
||
|
|
parent_orient = (_rot(a1, angle1) *
|
||
|
|
_rot(a2, angle2) *
|
||
|
|
_rot(a3, angle3))
|
||
|
|
else:
|
||
|
|
parent_orient = (_rot(a3, angle3) *
|
||
|
|
_rot(a2, angle2) *
|
||
|
|
_rot(a1, angle1))
|
||
|
|
parent_orient = parent_orient.T
|
||
|
|
|
||
|
|
obj = super().__new__(
|
||
|
|
cls, angle1, angle2, angle3, Str(rot_order))
|
||
|
|
obj._angle1 = angle1
|
||
|
|
obj._angle2 = angle2
|
||
|
|
obj._angle3 = angle3
|
||
|
|
obj._rot_order = original_rot_order
|
||
|
|
obj._parent_orient = parent_orient
|
||
|
|
|
||
|
|
return obj
|
||
|
|
|
||
|
|
@property
|
||
|
|
def angle1(self):
|
||
|
|
return self._angle1
|
||
|
|
|
||
|
|
@property
|
||
|
|
def angle2(self):
|
||
|
|
return self._angle2
|
||
|
|
|
||
|
|
@property
|
||
|
|
def angle3(self):
|
||
|
|
return self._angle3
|
||
|
|
|
||
|
|
@property
|
||
|
|
def rot_order(self):
|
||
|
|
return self._rot_order
|
||
|
|
|
||
|
|
|
||
|
|
class BodyOrienter(ThreeAngleOrienter):
|
||
|
|
"""
|
||
|
|
Class to denote a body-orienter.
|
||
|
|
"""
|
||
|
|
|
||
|
|
_in_order = True
|
||
|
|
|
||
|
|
def __new__(cls, angle1, angle2, angle3, rot_order):
|
||
|
|
obj = ThreeAngleOrienter.__new__(cls, angle1, angle2, angle3,
|
||
|
|
rot_order)
|
||
|
|
return obj
|
||
|
|
|
||
|
|
def __init__(self, angle1, angle2, angle3, rot_order):
|
||
|
|
"""
|
||
|
|
Body orientation takes this coordinate system through three
|
||
|
|
successive simple rotations.
|
||
|
|
|
||
|
|
Body fixed rotations include both Euler Angles and
|
||
|
|
Tait-Bryan Angles, see https://en.wikipedia.org/wiki/Euler_angles.
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
==========
|
||
|
|
|
||
|
|
angle1, angle2, angle3 : Expr
|
||
|
|
Three successive angles to rotate the coordinate system by
|
||
|
|
|
||
|
|
rotation_order : string
|
||
|
|
String defining the order of axes for rotation
|
||
|
|
|
||
|
|
Examples
|
||
|
|
========
|
||
|
|
|
||
|
|
>>> from sympy.vector import CoordSys3D, BodyOrienter
|
||
|
|
>>> from sympy import symbols
|
||
|
|
>>> q1, q2, q3 = symbols('q1 q2 q3')
|
||
|
|
>>> N = CoordSys3D('N')
|
||
|
|
|
||
|
|
A 'Body' fixed rotation is described by three angles and
|
||
|
|
three body-fixed rotation axes. To orient a coordinate system D
|
||
|
|
with respect to N, each sequential rotation is always about
|
||
|
|
the orthogonal unit vectors fixed to D. For example, a '123'
|
||
|
|
rotation will specify rotations about N.i, then D.j, then
|
||
|
|
D.k. (Initially, D.i is same as N.i)
|
||
|
|
Therefore,
|
||
|
|
|
||
|
|
>>> body_orienter = BodyOrienter(q1, q2, q3, '123')
|
||
|
|
>>> D = N.orient_new('D', (body_orienter, ))
|
||
|
|
|
||
|
|
is same as
|
||
|
|
|
||
|
|
>>> from sympy.vector import AxisOrienter
|
||
|
|
>>> axis_orienter1 = AxisOrienter(q1, N.i)
|
||
|
|
>>> D = N.orient_new('D', (axis_orienter1, ))
|
||
|
|
>>> axis_orienter2 = AxisOrienter(q2, D.j)
|
||
|
|
>>> D = D.orient_new('D', (axis_orienter2, ))
|
||
|
|
>>> axis_orienter3 = AxisOrienter(q3, D.k)
|
||
|
|
>>> D = D.orient_new('D', (axis_orienter3, ))
|
||
|
|
|
||
|
|
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.
|
||
|
|
|
||
|
|
>>> body_orienter1 = BodyOrienter(q1, q2, q3, '123')
|
||
|
|
>>> body_orienter2 = BodyOrienter(q1, q2, 0, 'ZXZ')
|
||
|
|
>>> body_orienter3 = BodyOrienter(0, 0, 0, 'XYX')
|
||
|
|
|
||
|
|
"""
|
||
|
|
# Dummy initializer for docstrings
|
||
|
|
pass
|
||
|
|
|
||
|
|
|
||
|
|
class SpaceOrienter(ThreeAngleOrienter):
|
||
|
|
"""
|
||
|
|
Class to denote a space-orienter.
|
||
|
|
"""
|
||
|
|
|
||
|
|
_in_order = False
|
||
|
|
|
||
|
|
def __new__(cls, angle1, angle2, angle3, rot_order):
|
||
|
|
obj = ThreeAngleOrienter.__new__(cls, angle1, angle2, angle3,
|
||
|
|
rot_order)
|
||
|
|
return obj
|
||
|
|
|
||
|
|
def __init__(self, angle1, angle2, angle3, rot_order):
|
||
|
|
"""
|
||
|
|
Space rotation is similar to Body rotation, but the rotations
|
||
|
|
are applied in the opposite order.
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
==========
|
||
|
|
|
||
|
|
angle1, angle2, angle3 : Expr
|
||
|
|
Three successive angles to rotate the coordinate system by
|
||
|
|
|
||
|
|
rotation_order : string
|
||
|
|
String defining the order of axes for rotation
|
||
|
|
|
||
|
|
See Also
|
||
|
|
========
|
||
|
|
|
||
|
|
BodyOrienter : Orienter to orient systems wrt Euler angles.
|
||
|
|
|
||
|
|
Examples
|
||
|
|
========
|
||
|
|
|
||
|
|
>>> from sympy.vector import CoordSys3D, SpaceOrienter
|
||
|
|
>>> from sympy import symbols
|
||
|
|
>>> q1, q2, q3 = symbols('q1 q2 q3')
|
||
|
|
>>> N = CoordSys3D('N')
|
||
|
|
|
||
|
|
To orient a coordinate system D with respect to N, each
|
||
|
|
sequential rotation is always about N's orthogonal unit vectors.
|
||
|
|
For example, a '123' rotation will specify rotations about
|
||
|
|
N.i, then N.j, then N.k.
|
||
|
|
Therefore,
|
||
|
|
|
||
|
|
>>> space_orienter = SpaceOrienter(q1, q2, q3, '312')
|
||
|
|
>>> D = N.orient_new('D', (space_orienter, ))
|
||
|
|
|
||
|
|
is same as
|
||
|
|
|
||
|
|
>>> from sympy.vector import AxisOrienter
|
||
|
|
>>> axis_orienter1 = AxisOrienter(q1, N.i)
|
||
|
|
>>> B = N.orient_new('B', (axis_orienter1, ))
|
||
|
|
>>> axis_orienter2 = AxisOrienter(q2, N.j)
|
||
|
|
>>> C = B.orient_new('C', (axis_orienter2, ))
|
||
|
|
>>> axis_orienter3 = AxisOrienter(q3, N.k)
|
||
|
|
>>> D = C.orient_new('C', (axis_orienter3, ))
|
||
|
|
|
||
|
|
"""
|
||
|
|
# Dummy initializer for docstrings
|
||
|
|
pass
|
||
|
|
|
||
|
|
|
||
|
|
class QuaternionOrienter(Orienter):
|
||
|
|
"""
|
||
|
|
Class to denote a quaternion-orienter.
|
||
|
|
"""
|
||
|
|
|
||
|
|
def __new__(cls, q0, q1, q2, q3):
|
||
|
|
q0 = sympify(q0)
|
||
|
|
q1 = sympify(q1)
|
||
|
|
q2 = sympify(q2)
|
||
|
|
q3 = sympify(q3)
|
||
|
|
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]]))
|
||
|
|
parent_orient = parent_orient.T
|
||
|
|
|
||
|
|
obj = super().__new__(cls, q0, q1, q2, q3)
|
||
|
|
obj._q0 = q0
|
||
|
|
obj._q1 = q1
|
||
|
|
obj._q2 = q2
|
||
|
|
obj._q3 = q3
|
||
|
|
obj._parent_orient = parent_orient
|
||
|
|
|
||
|
|
return obj
|
||
|
|
|
||
|
|
def __init__(self, angle1, angle2, angle3, rot_order):
|
||
|
|
"""
|
||
|
|
Quaternion orientation orients the new CoordSys3D 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.
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
==========
|
||
|
|
|
||
|
|
q0, q1, q2, q3 : Expr
|
||
|
|
The quaternions to rotate the coordinate system by
|
||
|
|
|
||
|
|
Examples
|
||
|
|
========
|
||
|
|
|
||
|
|
>>> from sympy.vector import CoordSys3D
|
||
|
|
>>> from sympy import symbols
|
||
|
|
>>> q0, q1, q2, q3 = symbols('q0 q1 q2 q3')
|
||
|
|
>>> N = CoordSys3D('N')
|
||
|
|
>>> from sympy.vector import QuaternionOrienter
|
||
|
|
>>> q_orienter = QuaternionOrienter(q0, q1, q2, q3)
|
||
|
|
>>> B = N.orient_new('B', (q_orienter, ))
|
||
|
|
|
||
|
|
"""
|
||
|
|
# Dummy initializer for docstrings
|
||
|
|
pass
|
||
|
|
|
||
|
|
@property
|
||
|
|
def q0(self):
|
||
|
|
return self._q0
|
||
|
|
|
||
|
|
@property
|
||
|
|
def q1(self):
|
||
|
|
return self._q1
|
||
|
|
|
||
|
|
@property
|
||
|
|
def q2(self):
|
||
|
|
return self._q2
|
||
|
|
|
||
|
|
@property
|
||
|
|
def q3(self):
|
||
|
|
return self._q3
|
||
|
|
|
||
|
|
|
||
|
|
def _rot(axis, angle):
|
||
|
|
"""DCM for simple axis 1, 2 or 3 rotations. """
|
||
|
|
if axis == 1:
|
||
|
|
return Matrix(rot_axis1(angle).T)
|
||
|
|
elif axis == 2:
|
||
|
|
return Matrix(rot_axis2(angle).T)
|
||
|
|
elif axis == 3:
|
||
|
|
return Matrix(rot_axis3(angle).T)
|