-
Notifications
You must be signed in to change notification settings - Fork 102
Expand file tree
/
Copy pathbaseposematrix.py
More file actions
1723 lines (1379 loc) · 61.1 KB
/
Copy pathbaseposematrix.py
File metadata and controls
1723 lines (1379 loc) · 61.1 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
# Part of Spatial Math Toolbox for Python
# Copyright (c) 2000 Peter Corke
# MIT Licence, see details in top-level file: LICENCE
from __future__ import annotations
import numpy as np
# try: # pragma: no cover
# # print('Using SymPy')
# from sympy.core.singleton import S
# _symbolics = True
# except ImportError: # pragma: no cover
# _symbolics = False
import spatialmath.base as smb
from typing_extensions import Self
from spatialmath.base.types import *
from spatialmath.baseposelist import BasePoseList
_eps = np.finfo(np.float64).eps
# colored printing of matrices to the terminal
# colored package has much finer control than colorama, but the latter is available by default with anaconda
try:
from colored import fg, bg, attr
_colored = True
# print('using colored output')
except ImportError:
# print('colored not found')
_colored = False
except AttributeError:
# print('colored failed to load, can happen from MATLAB import')
_colored = False
try:
from ansitable import ANSIMatrix
_ANSIMatrix = True
# print('using colored output')
except ImportError:
# print('colored not found')
_ANSIMatrix = False
class BasePoseMatrix(BasePoseList):
"""
Superclass for SO(N) and SE(N) objects
Subclasses are:
- ``SO2`` representing elements of SO(2) which describe rotations in 2D
- ``SE2`` representing elements of SE(2) which describe rigid-body motion in 2D
- ``SO3`` representing elements of SO(3) which describe rotations in 3D
- ``SE3`` representing elements of SE(3) which describe rigid-body motion in 3D
Arithmetic operators are overloaded but the operation they perform depend
on the types of the operands. For example:
- ``*`` will compose two instances of the same subclass, and the result will
be an instance of the same subclass, since this is a group operator.
- ``+`` will add two instances of the same subclass, and the result will be
a matrix, not an instance of the same subclass, since addition is not a group operator.
These classes all inherit from ``UserList`` which enables them to
represent a sequence of values, ie. an ``SE3`` instance can contain
a sequence of SE(3) values. Most of the Python ``list`` operators
are applicable::
>>> x = SE3() # new instance with identity matrix value
>>> len(x) # it is a sequence of one value
1
>>> x.append(x) # append to itself
>>> len(x) # it is a sequence of two values
2
>>> x[1] # the element has a 4x4 matrix value
SE3([
array([[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]) ])
>>> x[1] = SE3.Rx(0.3) # set an elements of the sequence
>>> x.reverse() # reverse the elements in the sequence
>>> del x[1] # delete an element
For console printing colorization is supported if the package ``colored``
is installed. Class variables control the colorization and can be assigned
to at any time.
=============== =================== ============================================
Variable Default Description
=============== =================== ============================================
_rotcolor 'red' Foreground color of rotation submatrix
_transcolor 'blue' Foreground color of rotation submatrix
_constcolor 'grey_50' Foreground color of matrix constant elements
_bgcolor None Background color of matrix
_indexcolor (None, 'yellow_2') Foreground, background color of index tag
_format '{:< 12g}' Format string for each matrix element
_suppress_small True Suppress *small* values, set to zero
_suppress_tol 100 Threshold for *small* values in eps units
_ansimatrix False Display with matrix brackets
=============== =================== ============================================
If color is specified as ``None`` it means no colorization is performed.
For example::
>> SE3._bgcolor = None
>> SE3._indexcolor = ('green', None)
.. note:: The ``_ansimatrix`` option requires that the ``ansitable`` package
is installed. It does not currently support colorization of elements.
"""
_rotcolor = "red"
_transcolor = "blue"
_bgcolor = None
_constcolor = "grey_50"
_indexcolor = (None, "yellow_2")
_format = "{:< 9.4g}"
_suppress_small = True
_suppress_tol = 100
_color = _colored
_ansimatrix = False
_ansiformatter = None
__array_ufunc__ = None # allow pose matrices operators with NumPy values
def __new__(cls, *args, **kwargs):
"""
Create the subclass instance (superclass method)
Create a new instance and call the superclass initializer to enable the
``UserList`` capabilities.
"""
pose = super(BasePoseMatrix, cls).__new__(cls) # create a new instance
super().__init__(pose) # initialize UserList
return pose
# ------------------------------------------------------------------------ #
@property
def about(self) -> str:
"""
Succinct summary of object type and length (superclass property)
:return: succinct summary
:rtype: str
Displays the type and the number of elements in compact form, for
example::
>>> x = SE3([SE3() for i in range(20)])
>>> len(x)
20
>>> print(x.about)
SE3[20]
"""
return "{:s}[{:d}]".format(type(self).__name__, len(self))
@property
def N(self) -> int:
"""
Dimension of the object's group (superclass property)
:return: dimension
:rtype: int
Dimension of the group is 2 for ``SO2`` or ``SE2``, and 3 for ``SO3`` or ``SE3``.
This corresponds to the dimension of the space, 2D or 3D, to which these
rotations or rigid-body motions apply.
Example::
>>> SE3().N
3
>>> SE2().N
2
"""
if type(self).__name__ == "SO2" or type(self).__name__ == "SE2":
return 2
else:
return 3
# ----------------------- tests
@property
def isSO(self) -> bool:
"""
Test if object belongs to SO(n) group (superclass property)
:param self: object to test
:type self: SO2, SE2, SO3, SE3 instance
:return: ``True`` if object is instance of SO2 or SO3
:rtype: bool
"""
return type(self).__name__ == "SO2" or type(self).__name__ == "SO3"
@property
def isSE(self) -> bool:
"""
Test if object belongs to SE(n) group (superclass property)
:param self: object to test
:type self: SO2, SE2, SO3, SE3 instance
:return: ``True`` if object is instance of SE2 or SE3
:rtype: bool
"""
return type(self).__name__ == "SE2" or type(self).__name__ == "SE3"
# ------------------------------------------------------------------------ #
# ------------------------------------------------------------------------ #
# --------- compatibility methods
def isrot(self) -> bool:
"""
Test if object belongs to SO(3) group (superclass method)
:return: ``True`` if object is instance of SO3
:rtype: bool
For compatibility with Spatial Math Toolbox for MATLAB.
In Python use ``isinstance(x, SO3)``.
Example::
>>> x = SO3()
>>> x.isrot()
True
>>> x = SE3()
>>> x.isrot()
False
"""
return type(self).__name__ == "SO3"
def isrot2(self) -> bool:
"""
Test if object belongs to SO(2) group (superclass method)
:return: ``True`` if object is instance of SO2
:rtype: bool
For compatibility with Spatial Math Toolbox for MATLAB.
In Python use ``isinstance(x, SO2)``.
Example::
>>> x = SO2()
>>> x.isrot()
True
>>> x = SE2()
>>> x.isrot()
False
"""
return type(self).__name__ == "SO2"
def ishom(self) -> bool:
"""
Test if object belongs to SE(3) group (superclass method)
:return: ``True`` if object is instance of SE3
:rtype: bool
For compatibility with Spatial Math Toolbox for MATLAB.
In Python use ``isinstance(x, SE3)``.
Example::
>>> x = SO3()
>>> x.isrot()
False
>>> x = SE3()
>>> x.isrot()
True
"""
return type(self).__name__ == "SE3"
def ishom2(self) -> bool:
"""
Test if object belongs to SE(2) group (superclass method)
:return: ``True`` if object is instance of SE2
:rtype: bool
For compatibility with Spatial Math Toolbox for MATLAB.
In Python use ``isinstance(x, SE2)``.
Example::
>>> x = SO2()
>>> x.isrot()
False
>>> x = SE2()
>>> x.isrot()
True
"""
return type(self).__name__ == "SE2"
# ----------------------- functions
def det(self) -> Tuple[float, Rn]:
"""
Determinant of rotational component (superclass method)
:return: Determinant of rotational component
:rtype: float or NumPy array
``x.det()`` is the determinant of the rotation component of the values
of ``x``.
Example::
>>> x=SE3.Rand()
>>> x.det()
1.0000000000000004
>>> x=SE3.Rand(N=2)
>>> x.det()
[0.9999999999999997, 1.0000000000000002]
:SymPy: not supported
"""
if type(self).__name__ in ("SO3", "SE3"):
if len(self) == 1:
return np.linalg.det(self.A[:3, :3])
else:
return [np.linalg.det(T[:3, :3]) for T in self.data]
elif type(self).__name__ in ("SO2", "SE2"):
if len(self) == 1:
return np.linalg.det(self.A[:2, :2])
else:
return [np.linalg.det(T[:2, :2]) for T in self.data]
def log(self, twist: Optional[bool] = False) -> Union[NDArray, List[NDArray]]:
"""
Logarithm of pose (superclass method)
:return: logarithm
:rtype: ndarray
:raises: ValueError
An efficient closed-form solution of the matrix logarithm.
===== ====== ===============================
Input Output
----- ---------------------------------------
Pose Shape Structure
===== ====== ===============================
SO2 (2,2) skew-symmetric SE2 (3,3) augmented skew-symmetric
SO3 (3,3) skew-symmetric SE3 (4,4) augmented skew-symmetric
===== ====== ===============================
Example::
>>> x = SE3.Rx(0.3)
>>> y = x.log()
>>> y
array([[ 0. , -0. , 0. , 0. ],
[ 0. , 0. , -0.3, 0. ],
[-0. , 0.3, 0. , 0. ],
[ 0. , 0. , 0. , 0. ]])
:seealso: :func:`~spatialmath.base.transforms2d.trlog2`,
:func:`~spatialmath.base.transforms3d.trlog`
:SymPy: not supported
"""
if self.N == 2:
log = [smb.trlog2(x, twist=twist) for x in self.data]
else:
log = [smb.trlog(x, twist=twist) for x in self.data]
if len(log) == 1:
return log[0]
else:
return log
def interp(
self,
end: Optional[bool] = None,
s: Union[int, float] = None,
shortest: bool = True,
) -> Self:
"""
Interpolate between poses (superclass method)
:param end: final pose
:type end: same as ``self``
:param s: interpolation coefficient, range 0 to 1, or number of steps
:type s: array_like or int
:param shortest: take the shortest path along the great circle for the rotation
:type shortest: bool, default to True
:return: interpolated pose
:rtype: same as ``self``
- ``X.interp(Y, s)`` interpolates pose between X between when s=0
and Y when s=1.
- ``X.interp(Y, N)`` interpolates pose between X and Y in ``N`` steps.
Example:
.. runblock:: pycon
>>> x = SE3(-1, -2, 0) * SE3.Rx(-0.3)
>>> y = SE3(1, 2, 0) * SE3.Rx(0.3)
>>> x.interp(y, 0) # this is x
>>> x.interp(y, 1) # this is y
>>> x.interp(y, 0.5) # this is in between
>>> z = x.interp(y, 11) # in 11 steps
>>> len(z)
>>> z[0] # this is x
>>> z[5] # this is in between
.. note::
- For SO3 and SE3 rotation is interpolated using quaternion spherical linear interpolation (slerp).
- Values of ``s`` outside the range [0,1] are silently clipped
:seealso: :func:`interp1`, :func:`~spatialmath.base.transforms3d.trinterp`, :func:`~spatialmath.base.quaternions.qslerp`, :func:`~spatialmath.base.transforms2d.trinterp2`
:SymPy: not supported
"""
if isinstance(s, int) and s > 1:
s = np.linspace(0, 1, s)
else:
s = smb.getvector(s)
s = np.clip(s, 0, 1)
if len(self) > 1:
raise ValueError("start pose must be a singleton")
if end is not None:
if len(end) > 1:
raise ValueError("end pose must be a singleton")
end = end.A
if self.N == 2:
# SO(2) or SE(2)
return self.__class__(
[
smb.trinterp2(start=self.A, end=end, s=_s, shortest=shortest)
for _s in s
]
)
elif self.N == 3:
# SO(3) or SE(3)
return self.__class__(
[
smb.trinterp(start=self.A, end=end, s=_s, shortest=shortest)
for _s in s
]
)
def interp1(self, s: float = None) -> Self:
"""
Interpolate pose (superclass method)
:param end: final pose
:type end: same as ``self``
:param s: interpolation coefficient, range 0 to 1
:type s: array_like
:return: interpolated pose
:rtype: SO2, SE2, SO3, SE3 instance
- ``X.interp(s)`` interpolates pose between identity when s=0, and X when s=1.
====== ====== =========== ===============================
len(X) len(s) len(result) Result
====== ====== =========== ===============================
1 1 1 Y = interp(X, s)
M 1 M Y[i] = interp(X[i], s)
1 M M Y[i] = interp(X, s[i])
====== ====== =========== ===============================
Example::
>>> x = SE3.Rx(0.3)
>>> print(x.interp(0))
SE3(array([[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]))
>>> print(x.interp(1))
SE3(array([[ 1. , 0. , 0. , 0. ],
[ 0. , 0.95533649, -0.29552021, 0. ],
[ 0. , 0.29552021, 0.95533649, 0. ],
[ 0. , 0. , 0. , 1. ]]))
>>> y = x.interp(x, np.linspace(0, 1, 10))
>>> len(y)
10
>>> y[5]
SE3(array([[ 1. , 0. , 0. , 0. ],
[ 0. , 0.98614323, -0.16589613, 0. ],
[ 0. , 0.16589613, 0.98614323, 0. ],
[ 0. , 0. , 0. , 1. ]]))
Notes:
#. For SO3 and SE3 rotation is interpolated using quaternion spherical linear interpolation (slerp).
:seealso: :func:`interp`, :func:`~spatialmath.base.transforms3d.trinterp`, :func:`~spatialmath.base.quaternions.qslerp`, :func:`~spatialmath.smb.transforms2d.trinterp2`
:SymPy: not supported
"""
s = smb.getvector(s)
s = np.clip(s, 0, 1)
if self.N == 2:
# SO(2) or SE(2)
if len(s) > 1:
assert len(self) == 1, "if len(s) > 1, len(X) must == 1"
return self.__class__([smb.trinterp2(start, self.A, s=_s) for _s in s])
else:
return self.__class__(
[smb.trinterp2(start, x, s=s[0]) for x in self.data]
)
elif self.N == 3:
# SO(3) or SE(3)
if len(s) > 1:
assert len(self) == 1, "if len(s) > 1, len(X) must == 1"
return self.__class__([smb.trinterp(None, self.A, s=_s) for _s in s])
else:
return self.__class__(
[smb.trinterp(None, x, s=s[0]) for x in self.data]
)
def norm(self) -> Self:
"""
Normalize pose (superclass method)
:return: pose
:rtype: SO2, SE2, SO3, SE3 instance
- ``X.norm()`` is an equivalent pose object but the rotational matrix
part of all values has been adjusted to ensure it is a proper orthogonal
matrix rotation.
Example::
>>> x = SE3()
>>> y = x.norm()
>>> y
SE3(array([[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]))
Notes:
#. Only the direction of A vector (the z-axis) is unchanged.
#. Used to prevent finite word length arithmetic causing transforms to
become 'unnormalized'.
:seealso: :func:`~spatialmath.base.transforms3d.trnorm`, :func:`~spatialmath.base.transforms2d.trnorm2`
"""
if self.N == 2:
return self.__class__([smb.trnorm2(x) for x in self.data])
else:
return self.__class__([smb.trnorm(x) for x in self.data])
def simplify(self) -> Self:
"""
Symbolically simplify matrix values (superclass method)
:return: pose with symbolic elements
:rtype: pose instance
Apply symbolic simplification to every element of every value in the
pose instance.
Example::
>>> a = SE3.Rx(sympy.symbols('theta'))
>>> b = a * a
>>> b
SE3(array([[1, 0, 0, 0.0],
[0, -sin(theta)**2 + cos(theta)**2, -2*sin(theta)*cos(theta), 0],
[0, 2*sin(theta)*cos(theta), -sin(theta)**2 + cos(theta)**2, 0],
[0.0, 0, 0, 1.0]], dtype=object)
>>> b.simplify()
SE3(array([[1, 0, 0, 0],
[0, cos(2*theta), -sin(2*theta), 0],
[0, sin(2*theta), cos(2*theta), 0],
[0, 0, 0, 1.00000000000000]], dtype=object))
.. todo:: No need to simplify the constants in bottom row
:SymPy: supported
"""
vf = np.vectorize(smb.sym.simplify)
return self.__class__([vf(x) for x in self.data], check=False)
def stack(self) -> NDArray:
"""
Convert to 3-dimensional matrix
:return: 3-dimensional NumPy array
:rtype: ndarray(n,n,m)
Converts the value to a 3-dimensional NumPy array where the values are
stacked along the third axis. The first two dimensions are given by
``self.shape``.
"""
return np.dstack(self.data)
def conjugation(self, A: NDArray) -> NDArray:
"""
Matrix conjugation
:param A: matrix to conjugate
:type A: ndarray
:return: conjugated matrix
:rtype: ndarray
Compute the conjugation :math:`\mat{X} \mat{A} \mat{X}^{-1}` where :math:`\mat{X}`
is the current object.
Example:
.. runblock:: pycon
>>> from spatialmath import SO2
>>> import numpy as np
>>> R = SO2(0.5)
>>> A = np.array([[10, 0], [0, 1]])
>>> print(R * A * R.inv())
>>> print(R.conjugation(A))
"""
if self.isSO:
return self.A @ A @ self.A.T
else:
return self.A @ A @ self.inv().A.T
# ----------------------- i/o stuff
def print(self, label: Optional[str] = None, file: Optional[TextIO] = None) -> None:
"""
Print pose as a matrix (superclass method)
:param label: label to print before the matrix, defaults to None
:type label: str, optional
:param file: file to write to, defaults to None
:type file: file object, optional
Print the pose as a matrix, with an optional line beforehand. By default
the matrix is printed to stdout.
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> SE3().print()
>>> SE3().print("pose is:")
:seealso: :meth:`printline` :meth:`strline`
"""
if label is not None:
print(label, file=file)
print(self, file=file)
def printline(self, *args, **kwargs) -> None:
r"""
Print pose in compact single line format (superclass method)
:param arg: value for orient option, optional
:type arg: str
:param label: text label to put at start of line
:type label: str
:param fmt: conversion format for each number as used by ``format()``
:type fmt: str
:param label: text label to put at start of line
:type label: str
:param orient: 3-angle convention to use, optional, ``SO3`` and ``SE3``
only
:type orient: str
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param file: file to write formatted string to. [default, stdout]
:type file: file object
Print pose in a compact single line format. If ``X`` has multiple
values, print one per line.
Orientation can be displayed in various formats:
============= =================================================
``orient`` description
============= =================================================
``'rpy/zyx'`` roll-pitch-yaw angles in ZYX axis order [default]
``'rpy/yxz'`` roll-pitch-yaw angles in YXZ axis order
``'rpy/zyx'`` roll-pitch-yaw angles in ZYX axis order
``'eul'`` Euler angles in ZYZ axis order
``'angvec'`` angle and axis
============= =================================================
Example:
.. runblock:: pycon
>>> from spatialmath import SE2, SE3
>>> x = SE3.Rx(0.3)
>>> x.printline()
>>> x = SE3.Rx([0.2, 0.3])
>>> x.printline()
>>> x.printline('angvec')
>>> x.printline(orient='angvec', fmt="{:.6f}")
>>> x = SE2(1, 2, 0.3)
>>> x.printline()
.. note::
- Default formatting is for compact display of data
- For tabular data set ``fmt`` to a fixed width format such as
``fmt='{:.3g}'``
:seealso: :meth:`strline` :func:`trprint`, :func:`trprint2`
"""
if self.N == 2:
for x in self.data:
smb.trprint2(x, *args, **kwargs)
else:
for x in self.data:
smb.trprint(x, *args, **kwargs)
def strline(self, *args, **kwargs) -> str:
"""
Convert pose to compact single line string (superclass method)
:param label: text label to put at start of line
:type label: str
:param fmt: conversion format for each number as used by ``format()``
:type fmt: str
:param label: text label to put at start of line
:type label: str
:param orient: 3-angle convention to use, optional, ``SO3`` and ``SE3``
only
:type orient: str
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: pose in string format
:rtype: str
Convert pose in a compact single line format. If ``X`` has multiple
values, the string has one pose per line.
Orientation can be displayed in various formats:
============= =================================================
``orient`` description
============= =================================================
``'rpy/zyx'`` roll-pitch-yaw angles in ZYX axis order [default]
``'rpy/yxz'`` roll-pitch-yaw angles in YXZ axis order
``'rpy/zyx'`` roll-pitch-yaw angles in ZYX axis order
``'eul'`` Euler angles in ZYZ axis order
``'angvec'`` angle and axis
============= =================================================
Example:
.. runblock:: pycon
>>> from spatialmath import SE2, SE3
>>> x = SE3.Rx(0.3)
>>> x.strline()
>>> x = SE3.Rx([0.2, 0.3])
>>> x.strline()
>>> x.strline('angvec')
>>> x.strline(orient='angvec', fmt="{:.6f}")
>>> x = SE2(1, 2, 0.3)
>>> x.strline()
.. note::
- Default formatting is for compact display of data
- For tabular data set ``fmt`` to a fixed width format such as
``fmt='{:.3g}'``
:seealso: :meth:`printline` :func:`trprint`, :func:`trprint2`
"""
s = ""
if self.N == 2:
for x in self.data:
s += smb.trprint2(x, *args, file=False, **kwargs)
else:
for x in self.data:
s += smb.trprint(x, *args, file=False, **kwargs)
return s
def __repr__(self) -> str:
"""
Readable representation of pose (superclass method)
:return: readable representation of the pose as a list of arrays
:rtype: str
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> x = SE3.Rx(0.3)
>>> repr(x)
"""
# TODO: really should iterate over all the elements, can have a symb
# element and ~eps values
def trim(x):
if x.dtype == "O":
return x
else:
return smb.removesmall(x)
name = type(self).__name__
if len(self) == 0:
return name + "([])"
elif len(self) == 1:
# need to indent subsequent lines of the native repr string by 4 spaces
return name + "(" + trim(self.A).__repr__().replace("\n", "\n ") + ")"
else:
# format this as a list of ndarrays
return (
name
+ "([\n"
+ ",\n".join([trim(v).__repr__() for v in self.data])
+ " ])"
)
def _repr_pretty_(self, p, cycle):
"""
Pretty string for IPython (superclass method)
:param p: pretty printer handle (ignored)
:param cycle: pretty printer flag (ignored)
Print colorized output when variable is displayed in IPython, ie. on a line by
itself.
Example::
In [1]: x
"""
# see https://ipython.org/ipython-doc/stable/api/generated/IPython.lib.pretty.html
if len(self) == 1:
p.text(str(self))
else:
for i, x in enumerate(self):
p.text(f"{i}:\n{str(x)}")
def __str__(self) -> str:
"""
Pretty string representation of pose (superclass method)
:return: readable representation of the pose
:rtype: str
Convert the pose's matrix value to a simple grid of numbers.
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> x = SE3.Rx(0.3)
>>> print(x)
Notes:
- By default, the output is colorised for an ANSI terminal console:
* red: rotational elements
* blue: translational elements
* white: constant elements
"""
if _ANSIMatrix and self._ansimatrix:
return self._string_matrix()
else:
return self._string_color(color=True)
def _string_matrix(self) -> str:
if self._ansiformatter is None:
self._ansiformatter = ANSIMatrix(style="thick")
return "\n".join([self._ansiformatter.str(A) for A in self.data])
def _string_color(self, color: Optional[bool] = False) -> str:
"""
Pretty print the matrix value
:param color: colorise the output, defaults to False
:type color: bool, optional
:return: multiline matrix representation
:rtype: str
Convert a matrix to a simple grid of numbers with optional
colorization for an ANSI terminal console:
* red: rotational elements
* blue: translational elements
* white: constant elements
"""
# print('in __str__', _color)
if self._color:
def color(c, f):
if c is None:
return ""
else:
return f(c)
bgcol = color(self._bgcolor, bg)
trcol = color(self._transcolor, fg) + bgcol
rotcol = color(self._rotcolor, fg) + bgcol
constcol = color(self._constcolor, fg) + bgcol
indexcol = color(self._indexcolor[0], fg) + color(self._indexcolor[1], bg)
reset = attr(0)
else:
bgcol = ""
trcol = ""
rotcol = ""
constcol = ""
reset = ""
def mformat(self, X):
# X is an ndarray value to be display
# self provides set type for formatting
out = ""
n = self.N # dimension of rotation submatrix
for rownum, row in enumerate(X):
rowstr = " "
# format the columns
for colnum, element in enumerate(row):
if smb.sym.issymbol(element):
s = "{:<12s}".format(str(element))
else:
if (
self._suppress_small
and abs(element) < self._suppress_tol * _eps
):
element = 0
s = self._format.format(element)
if rownum < n:
if colnum < n:
# rotation part
s = rotcol + bgcol + s + reset
else:
# translation part
s = trcol + bgcol + s + reset
else:
# bottom row
s = constcol + bgcol + s + reset
rowstr += " " + s
out += rowstr + bgcol + " " + reset + "\n"
return out
output_str = ""
if len(self.data) == 0:
output_str = "[]"
elif len(self.data) == 1:
# single matrix case
output_str = mformat(self, self.A)
else:
# sequence case
for count, X in enumerate(self.data):
# add separator lines and the index
output_str += (
indexcol
+ "[{:d}] =".format(count)
+ reset
+ "\n"
+ mformat(self, X)
)
return output_str
# ----------------------- graphics
def plot(self, *args, **kwargs) -> None:
"""
Plot pose object as a coordinate frame (superclass method)
:param `**kwargs`: plotting options
- ``X.plot()`` displays the pose ``X`` as a coordinate frame in either
2D or 3D. There are many options, see the links below.
Example::
>>> X = SE3.Rx(0.3)