8000 big push on quaternion class doco · flyinger/spatialmath-python@ba9b00b · GitHub
[go: up one dir, main page]
Skip to content

Commit ba9b00b

Browse files
8000
petercorkepetercorke
committed
big push on quaternion class doco
minor fixes along the way, consistency with pose3d
1 parent 321f642 commit ba9b00b

File tree

5 files changed

+1100
-266
lines changed

5 files changed

+1100
-266
lines changed

‎spatialmath/base/quaternions.py

Lines changed: 2 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -1,35 +1,5 @@
11
#!/usr/bin/env python3
2-
# -*- coding: utf-8 -*-
3-
"""
4-
Created on Fri Apr 10 14:12:56 2020
52

6-
@author: Peter Corke
7-
"""
8-
9-
# This file is part of the SpatialMath toolbox for Python
10-
# https://github.com/petercorke/spatialmath-python
11-
#
12-
# MIT License
13-
#
14-
# Copyright (c) 1993-2020 Peter Corke
15-
#
16-
# Permission is hereby granted, free of charge, to any person obtaining a copy
17-
# of this software and associated documentation files (the "Software"), to deal
18-
# in the Software without restriction, including without limitation the rights
19-
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
20-
# copies of the Software, and to permit persons to whom the Software is
21-
# furnished to do so, subject to the following conditions:
22-
#
23-
# The above copyright notice and this permission notice shall be included in all
24-
# copies or substantial portions of the Software.
25-
#
26-
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
27-
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
28-
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
29-
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
30-
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
31-
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
32-
# SOFTWARE.
333

344
# Contributors:
355
#
@@ -155,7 +125,7 @@ def isequal(q1, q2, tol=100, unitq=False):
155125
q1 = argcheck.getvector(q1, 4)
156126
q2 = argcheck.getvector(q2, 4)
157127

158-
if unit:
128+
if unitq:
159129
return (np.sum(np.abs(q1 - q2)) < tol * _eps) or (np.sum(np.abs(q1 + q2)) < tol * _eps)
160130
else:
161131
return (np.sum(np.abs(q1 - q2)) < tol * _eps)
@@ -574,7 +544,7 @@ def dotb(q, w):
574544
:return: rate of change of unit quaternion
575545
:rtype: numpy.ndarray, shape=(4,)
576546
577-
``dot(q, w)`` is the rate of change of the elements of the unit quaternion ``q``
547+
``dotb(q, w)`` is the rate of change of the elements of the unit quaternion ``q``
578548
which represents the orientation of a body frame with angular velocity ``w`` in
579549
the body frame.
580550

‎spatialmath/pose3d.py

Lines changed: 60 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,23 @@
88
from spatialmath import base as tr
99
from spatialmath import super_pose as sp
1010

11+
"""
12+
Classes to abstract 3D pose and orientation using matrices in SO(3) and SE(3)
13+
14+
To use::
15+
16+
from spatialmath.pose3d import *
17+
T = SE3.Rx(0.3)
18+
19+
import spatialmath as sm
20+
T = sm.SE3.Rx(0.3)
21+
22+
23+
.. inheritance-diagram:: spatialmath.pose3d
24+
:top-classes: collections.UserList
25+
:parts: 1
26+
"""
27+
1128
# ============================== SO3 =====================================#
1229

1330

@@ -18,7 +35,9 @@ class SO3(sp.SMPose):
1835
This subclass represents rotations in 3D space. Internally it is a 3x3 orthogonal matrix belonging
1936
to the group SO(3).
2037
21-
.. inheritance-diagram::
38+
.. inheritance-diagram:: spatialmath.pose3d.SO3
39+
:top-classes: collections.UserList
40+
:parts: 1
2241
"""
2342

2443
def __init__(self, arg=None, *, check=True):
@@ -186,6 +205,36 @@ def rpy(self, unit='deg', order='zyx'):
186205
else:
187206
return np.array([tr.tr2rpy(x, unit=unit, order=order) for x in self.A]).T
188207

208+
def angvec(self, unit='rad'):
209+
r"""
210+
SO(3) or SE(3) as angle and rotation vector
211+
212+
:param unit: angular units: 'rad' [default], or 'deg'
213+
:type unit: str
214+
:param check: check that rotation matrix is valid
215+
:type check: bool
216+
:return: :math:`(\theta, {\bf v})`
217+
:rtype: float, numpy.ndarray, shape=(3,)
218+
219+
``q.angvec()`` is a tuple :math:`(\theta, v)` containing the rotation
220+
angle and a rotation axis which is equivalent to the rotation of
221+
the unit quaternion ``q``.
222+
223+
By default the angle is in radians but can be changed setting `unit='deg'`.
224+
225+
Notes:
226+
227+
- If the input is SE(3) the translation component is ignored.
228+
229+
Example::
230+
231+
>>> UnitQuaternion.Rz(0.3).angvec()
232+
(0.3, array([0., 0., 1.]))
233+
234+
:seealso: :func:`~spatialmath.quaternion.AngVec`, :func:`~angvec2r`
235+
"""
236+
return tr.tr2angvec(self.R, unit=unit)
237+
189238
def Ad(self):
190239
"""
191240
Adjoint of SO(3)
@@ -486,6 +535,16 @@ def Exp(cls, S, check=True, so3=True):
486535

487536

488537
class SE3(SO3):
538+
"""
539+
SE(3) subclass
540+
541+
This subclass represents rigid-body motion in 3D space. Internally it is a 4x4 homogeneous transformation matrix belonging
542+
to the group SE(3).
543+
544+
.. inheritance-diagram:: spatialmath.pose3d.SE3
545+
:top-classes: collections.UserList
546+
:parts: 1
547+
"""
489548

490549
def __init__(self, x=None, y=None, z=None, *, check=True):
491550
"""
@@ -914,6 +973,5 @@ def Delta(cls, d):
914973
if __name__ == '__main__': # pragma: no cover
915974

916975
import pathlib
917-
import os.path
918976

919977
exec(open(os.path.join(pathlib.Path(__file__).parent.absolute(), "test_pose3d.py")).read())

0 commit comments

Comments
 (0)
0