8000 update doco for vector case · ZombyDogs/spatialmath-python@9fa4fdf · GitHub
[go: up one dir, main page]
Skip to content

Commit 9fa4fdf

Browse files
petercorkepetercorke
committed
update doco for vector case
update argcheck to allow check for matrix with wildcard dimension
1 parent 2760507 commit 9fa4fdf

File tree

3 files changed

+94
-36
lines changed

3 files changed

+94
-36
lines changed

‎spatialmath/base/argcheck.py

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,15 @@ def matrix(m, shape):
1414

1515

1616
def ismatrix(m, shape):
17-
return isinstance(m, np.ndarray) and m.shape == shape
17+
if not isinstance(m, np.ndarray):
18+
return False
19+
if len(shape) != len(m.shape):
20+
return False
21+
if shape[0] > 0 and shape[0] != m.shape[0]:
22+
return False
23+
if shape[1] > 0 and shape[1] != m.shape[1]:
24+
return False
25+
return True
1826

1927

2028
def verifymatrix(m, shape):

‎spatialmath/base/test_argcheck.py

Lines changed: 34 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -14,9 +14,21 @@
1414

1515

1616
class Test_check(unittest.TestCase):
17+
18+
def test_ismatrix(self):
19+
a = np.eye(3,3)
20+
self.assertTrue(ismatrix(a, (3,3)))
21+
self.assertFalse(ismatrix(a, (4,3)))
22+
self.assertFalse(ismatrix(a, (3,4)))
23+
self.assertFalse(ismatrix(a, (4,4)))
24+
25+
self.assertTrue(ismatrix(a, (-1,3)))
26+
self.assertTrue(ismatrix(a, (3,-1)))
27+
self.assertTrue(ismatrix(a, (-1,-1)))
28+
29+
self.assertFalse(ismatrix(1, (-1,-1)))
1730

1831
def test_unit(self):
19-
2032
nt.assert_equal(getunit(5, 'rad'), 5)
2133
nt.assert_equal(getunit(5, 'deg'), 5 * math.pi / 180.0)
2234
nt.assert_equal(getunit([3, 4, 5], 'rad'), [3, 4, 5])
@@ -29,31 +41,31 @@ def test_unit(self):
2941

3042
def test_isvector(self):
3143
# no length specified
32-
nt.assert_equal(isvector(2), True)
33-
nt.assert_equal(isvector(2.0), True)
34-
nt.assert_equal(isvector([1, 2, 3]), True)
35-
nt.assert_equal(isvector((1, 2, 3)), True)
36-
nt.assert_equal(isvector(np.array([1, 2, 3])), True)
37-
nt.assert_equal(isvector(np.array([[1, 2, 3]])), True)
38-
nt.assert_equal(isvector(np.array([[1], [2], [3]])), True)
44+
self.assertTrue(isvector(2))
45+
self.assertTrue(isvector(2.0))
46+
self.assertTrue(isvector([1, 2, 3]))
47+
self.assertTrue(isvector((1, 2, 3)))
48+
self.assertTrue(isvector(np.array([1, 2, 3])))
49+
self.assertTrue(isvector(np.array([[1, 2, 3]])))
50+
self.assertTrue(isvector(np.array([[1], [2], [3]])))
3951

4052
# length specified
41-
nt.assert_equal(isvector(2, 1), True)
42-
nt.assert_equal(isvector(2.0, 1), True)
43-
nt.assert_equal(isvector([1, 2, 3], 3), True)
44-
nt.assert_equal(isvector((1, 2, 3), 3), True)
45-
nt.assert_equal(isvector(np.array([1, 2, 3]), 3), True)
46-
nt.assert_equal(isvector(np.array([[1, 2, 3]]), 3), True)
47-
nt.assert_equal(isvector(np.array([[1], [2], [3]]), 3), True)
53+
self.assertTrue(isvector(2, 1))
54+
self.assertTrue(isvector(2.0, 1))
55+
self.assertTrue(isvector([1, 2, 3], 3))
56+
self.assertTrue(isvector((1, 2, 3), 3))
57+
self.assertTrue(isvector(np.array([1, 2, 3]), 3))
58+
self.assertTrue(isvector(np.array([[1, 2, 3]]), 3))
59+
self.assertTrue(isvector(np.array([[1], [2], [3]]), 3))
4860

4961
# wrong length specified
50-
nt.assert_equal(isvector(2, 4), False)
51-
nt.assert_equal(isvector(2.0, 4), False)
52-
nt.assert_equal(isvector([1, 2, 3], 4), False)
53-
nt.assert_equal(isvector((1, 2, 3), 4), False)
54-
nt.assert_equal(isvector(np.array([1, 2, 3]), 4), False)
55-
nt.assert_equal(isvector(np.array([[1, 2, 3]]), 4), False)
56-
nt.assert_equal(isvector(np.array([[1], [2], [3]]), 4), False)
62+
self.assertFalse(isvector(2, 4))
63+
self.assertFalse(isvector(2.0, 4))
64+
self.assertFalse(isvector([1, 2, 3], 4))
65+
self.assertFalse(isvector((1, 2, 3), 4))
66+
self.assertFalse(isvector(np.array([1, 2, 3]), 4))
67+
self.assertFalse(isvector(np.array([[1, 2, 3]]), 4))
68+
self.assertFalse(isvector(np.array([[1], [2], [3]]), 4))
5769

5870
def test_isvector(self):
5971
l = [1, 2, 3]

‎spatialmath/pose3d.py

Lines changed: 51 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -217,6 +217,9 @@ def Rx(cls, theta, unit='rad'):
217217
218218
- ``SE3.Rx(THETA)`` is an SO(3) rotation of THETA radians about the x-axis
219219
- ``SE3.Rx(THETA, "deg")`` as above but THETA is in degrees
220+
221+
If ``theta`` is an array then the result is a sequence of rotations defined by consecutive
222+
elements.
220223
"""
221224
return cls([tr.rotx(x, unit=unit) for x in argcheck.getvector(theta)], check=False)
222225

@@ -234,6 +237,9 @@ def Ry(cls, theta, unit='rad'):
234237
235238
- ``SO3.Ry(THETA)`` is an SO(3) rotation of THETA radians about the y-axis
236239
- ``SO3.Ry(THETA, "deg")`` as above but THETA is in degrees
240+
241+
If ``theta`` is an array then the result is a sequence of rotations defined by consecutive
242+
elements.
237243
"""
238244
return cls([tr.roty(x, unit=unit) for x in argcheck.getvector(theta)], check=False)
239245

@@ -248,9 +254,12 @@ def Rz(cls, theta, unit='rad'):
248254
:type unit: str
249255
:return: 3x3 rotation matrix
250256
:rtype: SO3 instance
251-
257+
252258
- ``SO3.Rz(THETA)`` is an SO(3) rotation of THETA radians about the z-axis
253259
- ``SO3.Rz(THETA, "deg")`` as above but THETA is in degrees
260+
261+
If ``theta`` is an array then the result is a sequence of rotations defined by consecutive
262+
elements.
254263
"""
255264
return cls([tr.rotz(x, unit=unit) for x in argcheck.getvector(theta)], check=False)
256265

@@ -265,8 +274,7 @@ def Rand(cls, N=1):
265274
:rtype: SO3 instance
266275
267276
- ``SO3.Rand()`` is a random SO(3) rotation.
268-
- ``SO3.Rand(N)`` is an SO3 object containing a sequence of N random
269-
rotations.
277+
- ``SO3.Rand(N)`` is a sequence of N random rotations.
270278
271279
:seealso: :func:`spatialmath.quaternion.UnitQuaternion.Rand`
272280
"""
@@ -278,14 +286,17 @@ def Eul(cls, angles, *, unit='rad'):
278286
Create an SO(3) rotation from Euler angles
279287
280288
:param angles: 3-vector of Euler angles
281-
:type angles: array_like
289+
:type angles: array_like or numpy.ndarray with shape=(N,3)
282290
:param unit: angular units: 'rad' [default], or 'deg'
283291
:type unit: str
284292
:return: 3x3 rotation matrix
285293
:rtype: SO3 instance
286294
287-
``SO3.Eul(ANGLES)`` is an SO(3) rotation defined by a 3-vector of Euler angles :math:`(\phi, \theta, \psi)` which
295+
``SO3.Eul(angles)`` is an SO(3) rotation defined by a 3-vector of Euler angles :math:`(\phi, \theta, \psi)` which
288296
correspond to consecutive rotations about the Z, Y, Z axes respectively.
297+
298+
If ``angles`` is an Nx3 matrix then the result is a sequence of rotations each defined by Euler angles
299+
correponding to the rows of angles.
289300
290301
:seealso: :func:`~spatialmath.pose3d.SE3.eul`, :func:`~spatialmath.pose3d.SE3.Eul`, :func:`spatialmath.base.transforms3d.eul2r`
291302
"""
@@ -300,15 +311,15 @@ def RPY(cls, angles, *, order='zyx', unit='rad'):
300311
Create an SO(3) rotation from roll-pitch-yaw angles
301312
302313
:param angles: 3-vector of roll-pitch-yaw angles
303-
:type angles: array_like
314+
:type angles: array_like or numpy.ndarray with shape=(N,3)
304315
:param unit: angular units: 'rad' [default], or 'deg'
305316
:type unit: str
306317
:param unit: rotation order: 'zyx' [default], 'xyz', or 'yxz'
307318
:type unit: str
308319
:return: 3x3 rotation matrix
309320
:rtype: SO3 instance
310321
311-
``SO3.RPY(ANGLES)`` is an SO(3) rotation defined by a 3-vector of roll, pitch, yaw angles :math:`(r, p, y)`
322+
``SO3.RPY(angles)`` is an SO(3) rotation defined by a 3-vector of roll, pitch, yaw angles :math:`(r, p, y)`
312323
which correspond to successive rotations about the axes specified by ``order``:
313324
314325
- 'zyx' [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
@@ -321,6 +332,9 @@ def RPY(cls, angles, *, order='zyx', unit='rad'):
321332
then by roll about the new z-axis. Convention for a camera with z-axis parallel
322333
to the optic axis and x-axis parallel to the pixel rows.
323334
335+
If ``angles`` is an Nx3 matrix then the result is a sequence of rotations each defined by RPY angles
336+
correponding to the rows of angles.
337+
324338
:seealso: :func:`~spatialmath.pose3d.SE3.rpy`, :func:`~spatialmath.pose3d.SE3.RPY`, :func:`spatialmath.base.transforms3d.rpy2r`
325339
"""
326340
if argcheck.isvector(angles, 3):
@@ -384,23 +398,32 @@ def AngVec(cls, theta, v, *, unit='rad'):
384398
return cls(tr.angvec2r(theta, v, unit=unit), check=False)
385399

386400
@classmethod
387-
def Exp(cls,S):
401+
def Exp(cls, S, so3=False):
388402
"""
389403
Create an SO(3) rotation matrix from so(3)
390404
391405
:param S: Lie algebra so(3)
392406
:type S: numpy ndarray
407+
:param so3: accept input as an so(3) matrix [default False]
408+
:type so3: bool
393409
:return: 3x3 rotation matrix
394410
:rtype: SO3 instance
395411
396412
- ``SO3.Exp(S)`` is an SO(3) rotation defined by its Lie algebra
397413
which is a 3x3 so(3) matrix (skew symmetric)
398414
- ``SO3.Exp(t)`` is an SO(3) rotation defined by a 3-element twist
399415
vector (the unique elements of the so(3) skew-symmetric matrix)
416+
- ``SO3.Exp(T)`` is a sequence of SO(3) rotations defined by an Nx3 matrix
417+
of twist vectors, one per row.
418+
419+
Note:
420+
421+
- an input 3x3 matrix is ambiguous, it could be the first or third case above. In this
422+
case the parameter `so3` is the decider.
400423
401424
:seealso: :func:`spatialmath.base.transforms3d.trexp`, :func:`spatialmath.base.transformsNd.skew`
402425
"""
403-
if isinstance(S, np.ndarray) and S.shape[1] == 3:
426+
if argcheck.ismatrix(S, (-1,3)) and not so3:
404427
return cls([tr.trexp(s) for s in S])
405428
else:
406429
return cls(tr.trexp(S), check=False)
@@ -492,9 +515,9 @@ def inv(self):
492515
:math:`T = \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]`
493516
"""
494517
if len(self) == 1:
495-
return SE3(tr.rt2tr(self.R.T, -self.R.T @ self.t))
518+
return SE3(tr.trinv(self.A))
496519
else:
497-
return SE3([tr.rt2tr(x.R.T, -x.R.T @ x.t) for x in self])
520+
return SE3([tr.trinv(x) for x in self.A])
498521

499522
@classmethod
500523
def isvalid(self, x):
@@ -525,6 +548,9 @@ def Rx(cls, theta, unit='rad'):
525548
526549
- ``SE3.Rx(THETA)`` is an SO(3) rotation of THETA radians about the x-axis
527550
- ``SE3.Rx(THETA, "deg")`` as above but THETA is in degrees
551+
552+
If ``theta`` is an array then the result is a sequence of rotations defined by consecutive
553+
elements.
528554
"""
529555
return cls([tr.trotx(x, unit) for x in argcheck.getvector(theta)])
530556

@@ -542,6 +568,9 @@ def Ry(cls, theta, unit='rad'):
542568
543569
- ``SE3.Ry(THETA)`` is an SO(3) rotation of THETA radians about the y-axis
544570
- ``SE3.Ry(THETA, "deg")`` as above but THETA is in degrees
571+
572+
If ``theta`` is an array then the result is a sequence of rotations defined by consecutive
573+
elements.
545574
"""
546575
return cls([tr.troty(x, unit) for x in argcheck.getvector(theta)])
547576

@@ -559,6 +588,9 @@ def Rz(cls, theta, unit='rad'):
559588
560589
- ``SE3.Rz(THETA)`` is an SO(3) rotation of THETA radians about the z-axis
561590
- ``SE3.Rz(THETA, "deg")`` as above but THETA is in degrees
591+
592+
If ``theta`` is an array then the result is a sequence of rotations defined by consecutive
593+
elements.
562594
"""
563595
return cls([tr.trotz(x, unit) for x in argcheck.getvector(theta)])
564596

@@ -590,14 +622,17 @@ def Eul(cls, angles, unit='rad'):
590622
Create an SE(3) pure rotation from Euler angles
591623
592624
:param angles: 3-vector of Euler angles
593-
:type angles: array_like
625+
:type angles: array_like or numpy.ndarray with shape=(N,3)
594626
:param unit: angular units: 'rad' [default], or 'deg'
595627
:type unit: str
596628
:return: 4x4 homogeneous transformation matrix
597629
:rtype: SE3 instance
598630
599631
``SE3.Eul(ANGLES)`` is an SO(3) rotation defined by a 3-vector of Euler angles :math:`(\phi, \theta, \psi)` which
600632
correspond to consecutive rotations about the Z, Y, Z axes respectively.
633+
634+
If ``angles`` is an Nx3 matrix then the result is a sequence of rotations each defined by Euler angles
635+
correponding to the rows of angles.
601636
602637
:seealso: :func:`~spatialmath.pose3d.SE3.eul`, :func:`~spatialmath.pose3d.SE3.Eul`, :func:`spatialmath.base.transforms3d.eul2r`
603638
"""
@@ -612,7 +647,7 @@ def RPY(cls, angles, order='zyx', unit='rad'):
612647
Create an SO(3) pure rotation from roll-pitch-yaw angles
613648
614649
:param angles: 3-vector of roll-pitch-yaw angles
615-
:type angles: array_like
650+
:type angles: array_like or numpy.ndarray with shape=(N,3)
616651
:param unit: angular units: 'rad' [default], or 'deg'
617652
:type unit: str
618653
:param unit: rotation order: 'zyx' [default], 'xyz', or 'yxz'
@@ -632,6 +667,9 @@ def RPY(cls, angles, order='zyx', unit='rad'):
632667
- 'yxz', rotate by yaw about the y-axis, then by pitch about the new x-axis,
633668
then by roll about the new z-axis. Convention for a camera with z-axis parallel
634669
to the optic axis and x-axis parallel to the pixel rows.
670+
671+
If ``angles`` is an Nx3 matrix then the result is a sequence of rotations each defined by RPY angles
672+
correponding to the rows of angles.
635673
636674
:seealso: :func:`~spatialmath.pose3d.SE3.rpy`, :func:`~spatialmath.pose3d.SE3.RPY`, :func:`spatialmath.base.transforms3d.rpy2r`
637675
"""

0 commit comments

Comments
 (0)
0