Repository navigation
Expand file tree
/
Copy pathpose2d.py
More file actions
498 lines (387 loc) · 16.7 KB
/
Copy pathpose2d.py
File metadata and controls
498 lines (387 loc) · 16.7 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
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Classes to abstract 2D pose and orientation using matrices in SE(2) and SO(2)
To use::
from spatialmath.pose2d import *
T = SE2(1, 2, 0.3)
import spatialmath as sm
T = sm.SE2.Rx(1, 2, 0.3)
.. inheritance-diagram:: spatialmath.pose3d
:top-classes: collections.UserList
:parts: 1
"""
# pylint: disable=invalid-name
import math
import numpy as np
from spatialmath.base import argcheck
from spatialmath import base as tr
from spatialmath.super_pose import SMPose
import spatialmath.pose3d as p3
# ============================== SO2 =====================================#
class SO2(SMPose):
"""
SO(2) matrix class
This subclass represents rotations in 2D space. Internally it is a 2x2 orthogonal matrix belonging
to the group SO(2).
.. inheritance-diagram:: spatialmath.pose2d.SO2
:top-classes: collections.UserList
:parts: 1
"""
# SO2() identity matrix
# SO2(angle, unit)
# SO2( obj ) # deep copy
# SO2( np ) # make numpy object
# SO2( nplist ) # make from list of numpy objects
# constructor needs to take ndarray -> SO2, or list of ndarray -> SO2
def __init__(self, arg=None, *, unit='rad', check=True):
"""
Construct new SO(2) object
:param unit: angular units 'deg' or 'rad' [default] if applicable
:type unit: str, optional
:param check: check for valid SO(2) elements if applicable, default to True
:type check: bool
:return: SO(2) rotation
:rtype: SO2 instance
- ``SO2()`` is an SO2 instance representing a null rotation -- the identity matrix.
- ``SO2(θ)`` is an SO2 instance representing a rotation by ``θ`` radians. If ``θ`` is array_like
`[θ1, θ2, ... θN]` then an SO2 instance containing a sequence of N rotations.
- ``SO2(θ, unit='deg')`` is an SO2 instance representing a rotation by ``θ`` degrees. If ``θ`` is array_like
`[θ1, θ2, ... θN]` then an SO2 instance containing a sequence of N rotations.
- ``SO2(R)`` is an SO2 instance with rotation described by the SO(2) matrix R which is a 2x2 numpy array. If ``check``
is ``True`` check the matrix belongs to SO(2).
- ``SO2([R1, R2, ... RN])`` is an SO2 instance containing a sequence of N rotations, each described by an SO(2) matrix
Ri which is a 2x2 numpy array. If ``check`` is ``True`` then check each matrix belongs to SO(2).
- ``SO2([X1, X2, ... XN])`` is an SO2 instance containing a sequence of N rotations, where each Xi is an SO2 instance.
"""
super().__init__()
if super().arghandler(arg, check=check):
return
elif argcheck.isscalar(arg):
self.data = [tr.rot2(arg, unit=unit)]
elif argcheck.isvector(arg):
self.data = [tr.rot2(x, unit=unit) for x in argcheck.getvector(arg)]
else:
raise ValueError('bad argument to constructor')
@staticmethod
def _identity():
return np.eye(2)
@property
def shape(self):
"""
Shape of the object's interal matrix representation
:return: (2,2)
:rtype: tuple
"""
return (2, 2)
@classmethod
def Rand(cls, *, arange=(0, 2 * math.pi), unit='rad', N=1):
r"""
Construct new SO(2) with random rotation
:param arange: rotation range, defaults to :math:`[0, 2\pi)`.
:type arange: 2-element array-like, optional
:param unit: angular units as 'deg or 'rad' [default]
:type unit: str, optional
:param N: number of random rotations, defaults to 1
:type N: int
:return: SO(2) rotation matrix
:rtype: SO2 instance
- ``SO2.Rand()`` is a random SO(2) rotation.
- ``SO2.Rand([-90, 90], unit='deg')`` is a random SO(2) rotation between
-90 and +90 degrees.
- ``SO2.Rand(N)`` is a sequence of N random rotations.
Rotations are uniform over the specified interval.
"""
rand = np.random.uniform(low=arange[0], high=arange[1], size=N) # random values in the range
return cls([tr.rot2(x) for x in argcheck.getunit(rand, unit)])
@classmethod
def Exp(cls, S, check=True):
"""
Construct new SO(2) rotation matrix from so(2) Lie algebra
:param S: element of Lie algebra so(2)
:type S: numpy ndarray
:param check: check that passed matrix is valid so(2), default True
:type check: bool
:return: SO(2) rotation matrix
:rtype: SO2 instance
- ``SO2.Exp(S)`` is an SO(2) rotation defined by its Lie algebra
which is a 2x2 so(2) matrix (skew symmetric)
:seealso: :func:`spatialmath.base.transforms2d.trexp`, :func:`spatialmath.base.transformsNd.skew`
"""
if isinstance(S, (list, tuple)):
return cls([tr.trexp2(s, check=check) for s in S])
else:
return cls(tr.trexp2(S, check=check), check=False)
@staticmethod
def isvalid(x, check=True):
"""
Test if matrix is valid SO(2)
:param x: matrix to test
:type x: numpy.ndarray
:return: True if the matrix is a valid element of SO(2), ie. it is a 2x2
orthonormal matrix with determinant of +1.
:rtype: bool
:seealso: :func:`~spatialmath.base.transform3d.isrot`
"""
return not check or tr.isrot2(x, check=True)
def inv(self):
"""
Inverse of SO(2)
:return: inverse rotation
:rtype: SO2 instance
- ``x.inv()`` is the inverse of `x`.
Notes:
- for elements of SO(2) this is the transpose.
- if `x` contains a sequence, returns an `SO2` with a sequence of inverses
"""
if len(self) == 1:
return SO2(self.A.T)
else:
return SO2([x.T for x in self.A])
@property
def R(self):
"""
SO(2) or SE(2) as rotation matrix
:return: rotational component
:rtype: numpy.ndarray, shape=(2,2)
``x.R`` returns the rotation matrix, when `x` is `SO2` or `SE2`. If `len(x)` is:
- 1, return an ndarray with shape=(2,2)
- N>1, return ndarray with shape=(N,2,2)
"""
return self.A[:2, :2]
def theta(self, unit='rad'):
"""
SO(2) as a rotation angle
:param unit: angular units 'deg' or 'rad' [default]
:type unit: str, optional
:return: rotation angle
:rtype: float or list
``x.theta`` is the rotation angle such that `x` is `SO2(x.theta)`.
"""
if unit == 'deg':
conv = 180.0 / math.pi
else:
conv = 1.0
if len(self) == 1:
return conv * math.atan2(self.A[1, 0], self.A[0, 0])
else:
return [conv * math.atan2(x.A[1, 0], x.A[0, 0]) for x in self]
def SE2(self):
"""
Create SE(2) from SO(2)
:return: SE(2) with same rotation but zero translation
:rtype: SE2 instance
"""
return SE2(tr.rt2tr(self.A, [0, 0]))
# ============================== SE2 =====================================#
class SE2(SO2):
"""
SE(2) matrix class
This subclass represents rigid-body motion (pose) in 2D space. Internally
it is a 3x3 homogeneous transformation matrix belonging to the group SE(2).
.. inheritance-diagram:: spatialmath.pose2d.SE2
:top-classes: collections.UserList
:parts: 1
"""
# constructor needs to take ndarray -> SO2, or list of ndarray -> SO2
def __init__(self, x=None, y=None, theta=None, *, unit='rad', check=True):
"""
Construct new SE(2) object
:param unit: angular units 'deg' or 'rad' [default] if applicable :type
unit: str, optional :param check: check for valid SE(2) elements if
applicable, default to True :type check: bool :return: homogeneous
rigid-body transformation matrix :rtype: SE2 instance
- ``SE2()`` is an SE2 instance representing a null motion -- the
identity matrix
- ``SE2(θ)`` is an SE2 instance representing a pure rotation of
``θ`` radians
- ``SE2(θ, unit='deg')`` as above but ``θ`` in degrees
- ``SE2(x, y)`` is an SE2 instance representing a pure translation of
(``x``, ``y``)
- ``SE2(t)`` is an SE2 instance representing a pure translation of
(``x``, ``y``) where``t``=[x,y] is a 2-element array_like
- ``SE2(x, y, θ)`` is an SE2 instance representing a translation of
(``x``, ``y``) and a rotation of ``θ`` radians
- ``SE2(x, y, θ, unit='deg')`` as above but ``θ`` in degrees
- ``SE2(t)`` where ``t``=[x,y] is a 2-element array_like, is an SE2
instance representing a pure translation of (``x``, ``y``)
- ``SE2(q)`` where ``q``=[x,y,θ] is a 3-element array_like, is an SE2
instance representing a translation of (``x``, ``y``) and a rotation
of ``θ`` radians
- ``SE2(t, unit='deg')`` as above but ``θ`` in degrees
- ``SE2(T)`` is an SE2 instance with rigid-body motion described by the
SE(2) matrix T which is a 3x3 numpy array. If ``check`` is ``True``
check the matrix belongs to SE(2).
- ``SE2([T1, T2, ... TN])`` is an SE2 instance containing a sequence of
N rigid-body motions, each described by an SE(2) matrix Ti which is a
3x3 numpy array. If ``check`` is ``True`` then check each matrix
belongs to SE(2).
- ``SE2([X1, X2, ... XN])`` is an SE2 instance containing a sequence of
N rigid-body motions, where each Xi is an SE2 instance.
"""
if y is None and theta is None:
# just one argument passed
if super().arghandler(x, check=check):
return
elif argcheck.isscalar(x):
self.data = [tr.trot2(x, unit=unit)]
elif len(x) == 2:
# SE2([x,y])
self.data = [tr.transl2(x)]
elif len(x) == 3:
# SE2([x,y,theta])
self.data = [tr.trot2(x[2], t=x[:2], unit=unit)]
else:
raise ValueError('bad argument to constructor')
elif x is not None:
if y is not None and theta is None:
# SE2(x, y)
self.data = [tr.transl2(x, y)]
elif y is not None and theta is not None:
# SE2(x, y, theta)
self.data = [tr.trot2(theta, t=[x, y], unit=unit)]
else:
raise ValueError('bad arguments to constructor')
@staticmethod
def _identity():
return np.eye(3)
@property
def shape(self):
"""
Shape of the object's interal matrix representation
:return: (3,3)
:rtype: tuple
"""
return (3, 3)
@classmethod
def Rand(cls, *, xrange=(-1, 1), yrange=(-1, 1), arange=(0, 2 * math.pi), unit='rad', N=1): # pylint: disable=arguments-differ
r"""
Construct a new random SE(2)
:param xrange: x-axis range [min,max], defaults to [-1, 1]
:type xrange: 2-element sequence, optional
:param yrange: y-axis range [min,max], defaults to [-1, 1]
:type yrange: 2-element sequence, optional
:param arange: angle range [min,max], defaults to :math:`[0, 2\pi)`
:type arange: 2-element sequence, optional
:param N: number of random rotations, defaults to 1
:type N: int
:param unit: angular units 'deg' or 'rad' [default] if applicable
:type unit: str, optional
:return: homogeneous rigid-body transformation matrix
:rtype: SE2 instance
Return an SE2 instance with random rotation and translation.
- ``SE2.Rand()`` is a random SE(2) rotation.
- ``SE2.Rand(N)`` is an SE2 object containing a sequence of N random
poses.
Example, create random ten vehicles in the xy-plane::
>>> x = SE3.Rand(N=10, xrange=[-2,2], yrange=[-2,2])
>>> len(x)
10
"""
x = np.random.uniform(low=xrange[0], high=xrange[1], size=N) # random values in the range
y = np.random.uniform(low=yrange[0], high=yrange[1], size=N) # random values in the range
theta = np.random.uniform(low=arange[0], high=arange[1], size=N) # random values in the range
return cls([tr.trot2(t, t=[x, y]) for (t, x, y) in zip(x, y, argcheck.getunit(theta, unit))])
@classmethod
def Exp(cls, S, check=True): # pylint: disable=arguments-differ
"""
Construct a new SE(2) from se(2) Lie algebra
:param S: element of Lie algebra se(2)
:type S: numpy ndarray
:param check: check that passed matrix is valid se(2), default True
:type check: bool
:return: homogeneous transform matrix
:rtype: SE2 instance
- ``SE2.Exp(S)`` is an SE(2) rotation defined by its Lie algebra
which is a 3x3 se(2) matrix (skew symmetric)
- ``SE2.Exp(t)`` is an SE(2) rotation defined by a 3-element twist
vector array_like (the unique elements of the se(2) skew-symmetric matrix)
- ``SE2.Exp(T)`` is a sequence of SE(2) rigid-body motions defined by an Nx3 matrix of twist vectors, one per row.
Note:
- an input 3x3 matrix is ambiguous, it could be the first or third case above. In this case the argument ``se2`` is the decider.
:seealso: :func:`spatialmath.base.transforms2d.trexp`, :func:`spatialmath.base.transformsNd.skew`
"""
if isinstance(S, (list, tuple)):
return cls([tr.trexp2(s) for s in S])
else:
return cls(tr.trexp2(S), check=False)
@staticmethod
def isvalid(x, check=True):
"""
Test if matrix is valid SE(2)
:param x: matrix to test
:type x: numpy.ndarray
:return: true if the matrix is a valid element of SE(2), ie. it is a
3x3 homogeneous rigid-body transformation matrix.
:rtype: bool
:seealso: :func:`~spatialmath.base.transform2d.ishom`
"""
return not check or tr.ishom2(x, check=True)
@property
def t(self):
"""
Translational component of SE(2)
:param self: SE(2)
:type self: SE2 instance
:return: translational component
:rtype: numpy.ndarray
``x.t`` is the translational vector component. If ``len(x)`` is:
- 1, return an ndarray with shape=(2,)
- N>1, return an ndarray with shape=(N,2)
"""
if len(self) == 1:
return self.A[:2, 2]
else:
return np.array([x[:2, 2] for x in self.A])
def xyt(self):
r"""
SE(2) as a configuration vector
:return: An array :math:`[x, y, \theta]` :rtype: numpy.ndarray
``x.xyt`` is the rigidbody motion in minimal form as a translation and
rotation expressed in vector form as :math:`[x, y, \theta]`. If
``len(x)`` is:
- 1, return an ndarray with shape=(3,)
- N>1, return an ndarray with shape=(N,3)
"""
if len(self) == 1:
return np.r_[self.t, self.theta()]
else:
return [np.r_[x.t, x.theta()] for x in self]
def inv(self):
r"""
Inverse of SE(2)
:param self: pose
:type self: SE2 instance
:return: inverse
:rtype: SE2
Notes:
- for elements of SE(2) this takes into account the matrix structure :math:`T^{-1} = \left[ \begin{array}{cc} R & t \\ 0 & 1 \end{array} \right], T^{-1} = \left[ \begin{array}{cc} R^T & -R^T t \\ 0 & 1 \end{array} \right]`
- if `x` contains a sequence, returns an `SE2` with a sequence of inverses
"""
if len(self) == 1:
return SE2(tr.rt2tr(self.R.T, -self.R.T @ self.t))
else:
return SE2([tr.rt2tr(x.R.T, -x.R.T @ x.t) for x in self])
def SE3(self, z=0):
"""
Create SE(3) from SE(2)
:param z: default z coordinate, defaults to 0
:type z: float
:return: SE(2) with same rotation but zero translation
:rtype: SE2 instance
"Lifts" 2D rigid-body motion to 3D, rotation in the xy-plane (about the z-axis) and
z-coordinate is settable.
"""
def lift3(x):
y = np.eye(4)
y[:2, :2] = x.A[:2, :2]
y[:2, 3] = x.A[:2, 2]
y[2, 3] = z
return y
return p3.SE3([lift3(x) for x in self])
def Twist2(self):
from spatialmath.twist import Twist2
return Twist2(self.log(twist=True))
if __name__ == '__main__': # pragma: no cover
import pathlib
exec(open(pathlib.Path(__file__).parent.parent.absolute() / "tests" / "test_pose2d.py").read()) # pylint: disable=exec-used