Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly
Sign up
Appearance settings

Commit a08f25e

Browse files
Add q2str to convert quaternion to string (#158)
1 parent 8c6d422 commit a08f25e

File tree

4 files changed

+79
-25
lines changed

4 files changed

+79
-25
lines changed

‎spatialmath/base/__init__.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -208,6 +208,7 @@
208208
"qdotb",
209209
"qangle",
210210
"qprint",
211+
"q2str",
211212
# spatialmath.base.transforms2d
212213
"rot2",
213214
"trot2",

‎spatialmath/base/quaternions.py‎

Lines changed: 52 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -19,9 +19,11 @@
1919
import scipy.interpolate as interpolate
2020
from typing import Optional
2121
from functools import lru_cache
22+
import warnings
2223

2324
_eps = np.finfo(np.float64).eps
2425

26+
2527
def qeye() -> QuaternionArray:
2628
"""
2729
Create an identity quaternion
@@ -56,7 +58,7 @@ def qpure(v: ArrayLike3) -> QuaternionArray:
5658
5759
.. runblock:: pycon
5860
59-
>>> from spatialmath.base import pure, qprint
61+
>>> from spatialmath.base import qpure, qprint
6062
>>> q = qpure([1, 2, 3])
6163
>>> qprint(q)
6264
"""
@@ -1088,14 +1090,53 @@ def qangle(q1: ArrayLike4, q2: ArrayLike4) -> float:
10881090
return 4.0 * math.atan2(smb.norm(q1 - q2), smb.norm(q1 + q2))
10891091

10901092

1093+
def q2str(
1094+
q: Union[ArrayLike4, ArrayLike4],
1095+
delim: Optional[Tuple[str, str]] = ("<", ">"),
1096+
fmt: Optional[str] = "{: .4f}",
1097+
) -> str:
1098+
"""
1099+
Format a quaternion as a string
1100+
1101+
:arg q: unit-quaternion
1102+
:type q: array_like(4)
1103+
:arg delim: 2-list of delimeters [default ('<', '>')]
1104+
:type delim: list or tuple of strings
1105+
:arg fmt: printf-style format soecifier [default '{: .4f}']
1106+
:type fmt: str
1107+
:return: formatted string
1108+
:rtype: str
1109+
1110+
Format the quaternion in a human-readable form as::
1111+
1112+
S D1 VX VY VZ D2
1113+
1114+
where S, VX, VY, VZ are the quaternion elements, and D1 and D2 are a pair
1115+
of delimeters given by `delim`.
1116+
1117+
.. runblock:: pycon
1118+
1119+
>>> from spatialmath.base import q2str, qrand
1120+
>>> q = [1, 2, 3, 4]
1121+
>>> q2str(q)
1122+
>>> q = qrand() # a unit quaternion
1123+
>>> q2str(q, delim=('<<', '>>'))
1124+
1125+
:seealso: :meth:`qprint`
1126+
"""
1127+
q = smb.getvector(q, 4)
1128+
template = "# {} #, #, # {}".replace("#", fmt)
1129+
return template.format(q[0], delim[0], q[1], q[2], q[3], delim[1])
1130+
1131+
10911132
def qprint(
10921133
q: Union[ArrayLike4, ArrayLike4],
10931134
delim: Optional[Tuple[str, str]] = ("<", ">"),
10941135
fmt: Optional[str] = "{: .4f}",
10951136
file: Optional[TextIO] = sys.stdout,
1096-
) -> str:
1137+
) -> None:
10971138
"""
1098-
Format a quaternion
1139+
Format a quaternion to a file
10991140
11001141
:arg q: unit-quaternion
11011142
:type q: array_like(4)
@@ -1105,8 +1146,6 @@ def qprint(
11051146
:type fmt: str
11061147
:arg file: destination for formatted string [default sys.stdout]
11071148
:type file: file object
1108-
:return: formatted string
1109-
:rtype: str
11101149
11111150
Format the quaternion in a human-readable form as::
11121151
@@ -1117,23 +1156,23 @@ def qprint(
11171156
11181157
By default the string is written to `sys.stdout`.
11191158
1120-
If `file=None` then a string is returned.
1121-
11221159
.. runblock:: pycon
11231160
11241161
>>> from spatialmath.base import qprint, qrand
11251162
>>> q = [1, 2, 3, 4]
11261163
>>> qprint(q)
11271164
>>> q = qrand() # a unit quaternion
11281165
>>> qprint(q, delim=('<<', '>>'))
1166+
1167+
:seealso: :meth:`q2str`
11291168
"""
11301169
q = smb.getvector(q, 4)
1131-
template="# {} #, #, # {}".replace("#", fmt)
1132-
s=template.format(q[0], delim[0], q[1], q[2], q[3], delim[1])
1133-
iffile:
1134-
file.write(s+"\n")
1135-
else:
1136-
returns
1170+
iffileisNone:
1171+
warnings.warn(
1172+
"Usage: qprint(..., file=None) -> str is deprecated, use q2str() instead",
1173+
DeprecationWarning,
1174+
)
1175+
print(q2str(q, delim=delim, fmt=fmt), file=file)
11371176

11381177

11391178
if __name__ == "__main__": # pragma: no cover

‎spatialmath/quaternion.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -920,7 +920,7 @@ def __str__(self) -> str:
920920
delim = ("<<", ">>")
921921
else:
922922
delim = ("<", ">")
923-
return "\n".join([smb.qprint(q, file=None, delim=delim) for q in self.data])
923+
return "\n".join([smb.q2str(q, delim=delim) for q in self.data])
924924

925925

926926
# ========================================================================= #

‎tests/base/test_quaternions.py‎

Lines changed: 25 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@
3636
import spatialmath.base as tr
3737
from spatialmath.base.quaternions import *
3838
import spatialmath as sm
39+
import io
3940

4041

4142
class TestQuaternion(unittest.TestCase):
@@ -96,19 +97,32 @@ def test_ops(self):
9697
),
9798
True,
9899
)
100+
nt.assert_equal(isunitvec(qrand()), True)
99101

100-
s = qprint(np.r_[1, 1, 0, 0], file=None)
101-
nt.assert_equal(isinstance(s, str), True)
102-
nt.assert_equal(len(s) > 2, True)
103-
s = qprint([1, 1, 0, 0], file=None)
102+
def test_display(self):
103+
s = q2str(np.r_[1, 2, 3, 4])
104104
nt.assert_equal(isinstance(s, str), True)
105-
nt.assert_equal(len(s) > 2, True)
105+
nt.assert_equal(s, " 1.0000 < 2.0000, 3.0000, 4.0000 >")
106+
107+
s = q2str([1, 2, 3, 4])
108+
nt.assert_equal(s, " 1.0000 < 2.0000, 3.0000, 4.0000 >")
106109

110+
s = q2str([1, 2, 3, 4], delim=("<<", ">>"))
111+
nt.assert_equal(s, " 1.0000 << 2.0000, 3.0000, 4.0000 >>")
112+
113+
s = q2str([1, 2, 3, 4], fmt="{:20.6f}")
107114
nt.assert_equal(
108-
qprint([1, 2, 3, 4], file=None), " 1.0000 < 2.0000, 3.0000, 4.0000 >"
115+
s,
116+
" 1.000000 < 2.000000, 3.000000, 4.000000 >",
109117
)
110118

111-
nt.assert_equal(isunitvec(qrand()), True)
119+
# would be nicer to do this with redirect_stdout() from contextlib but that
120+
# fails because file=sys.stdout is maybe assigned at compile time, so when
121+
# contextlib changes sys.stdout, qprint() doesn't see it
122+
123+
f = io.StringIO()
124+
qprint(np.r_[1, 2, 3, 4], file=f)
125+
nt.assert_equal(f.getvalue().rstrip(), " 1.0000 < 2.0000, 3.0000, 4.0000 >")
112126

113127
def test_rotation(self):
114128
# rotation matrix to quaternion
@@ -227,12 +241,12 @@ def test_r2q(self):
227241

228242
def test_qangle(self):
229243
# Test function that calculates angle between quaternions
230-
q1 = [1., 0, 0, 0]
231-
q2 = [1 / np.sqrt(2), 0, 1 / np.sqrt(2), 0] # 90deg rotation about y-axis
244+
q1 = [1.0, 0, 0, 0]
245+
q2 = [1 / np.sqrt(2), 0, 1 / np.sqrt(2), 0] # 90deg rotation about y-axis
232246
nt.assert_almost_equal(qangle(q1, q2), np.pi / 2)
233247

234-
q1 = [1., 0, 0, 0]
235-
q2 = [1 / np.sqrt(2), 1 / np.sqrt(2), 0, 0] # 90deg rotation about x-axis
248+
q1 = [1.0, 0, 0, 0]
249+
q2 = [1 / np.sqrt(2), 1 / np.sqrt(2), 0, 0] # 90deg rotation about x-axis
236250
nt.assert_almost_equal(qangle(q1, q2), np.pi / 2)
237251

238252

0 commit comments

Comments
(0)

AltStyle によって変換されたページ (->オリジナル) /