def test_Rt(self):
nt.assert_array_almost_equal(rot2(0.3), t2r(trot2(0.3)))
nt.assert_array_almost_equal(trot2(0.3), r2t(rot2(0.3)))
R = rot2(0.2)
t = [1, 2]
T = rt2tr(R, t)
nt.assert_array_almost_equal(t2r(T), R)
nt.assert_array_almost_equal(transl2(T), np.array(t))
def trinterp2(T0, T1=None, s=None):
"""
Interpolate SE(2) matrices
:param T0: first SE(2) matrix
:type T0: np.ndarray, shape=(3,3)
:param T1: second SE(2) matrix
:type T1: np.ndarray, shape=(3,3)
:param s: interpolation coefficient, range 0 to 1
:type s: float
:return: SE(2) matrix
:rtype: np.ndarray, shape=(3,3)
- ``trinterp2(T0, T1, S)`` is a homogeneous transform (3x3) interpolated
between T0 when S=0 and T1 when S=1. T0 and T1 are both homogeneous
transforms (3x3).
- ``trinterp2(T1, S)`` as above but interpolated between the identity matrix
when S=0 to T1 when S=1.
Notes::
- Rotation angle is linearly interpolated.
:seealso: :func:`~spatialmath.base.transforms3d.trinterp`
%## 2d homogeneous trajectory
"""
if T1 is None:
# TRINTERP2(T, s)
th0 = math.atan2(T0[1, 0], T0[0, 0])
p0 = transl2(T0)
th = s * th0
pr = s * p0
else:
# TRINTERP2(T0, T1, s)
th0 = math.atan2(T0[1, 0], T0[0, 0])
th1 = math.atan2(T1[1, 0], T1[0, 0])
p0 = transl2(T0)
p1 = transl2(T1)
pr = p0 * (1 - s) + s * p1
th = th0 * (1 - s) + s * th1
return trn.rt2tr(rot2(th), pr)
def trexp2(S, theta=None):
"""
Exponential of so(2) or se(2) matrix
:param S: so(2), se(2) matrix or equivalent velctor
:type T: numpy.ndarray, shape=(2,2) or (3,3); array_like
:param theta: motion
:type theta: float
:return: 2x2 or 3x3 matrix exponential in SO(2) or SE(2)
:rtype: numpy.ndarray, shape=(2,2) or (3,3)
An efficient closed-form solution of the matrix exponential for arguments
that are so(2) or se(2).
For so(2) the results is an SO(2) rotation matrix:
- ``trexp2(S)`` is the matrix exponential of the so(3) element ``S`` which is a 2x2
skew-symmetric matrix.
- ``trexp2(S, THETA)`` as above but for an so(3) motion of S*THETA, where ``S`` is
unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude
given by ``THETA``.
- ``trexp2(W)`` is the matrix exponential of the so(2) element ``W`` expressed as
a 1-vector (array_like).
- ``trexp2(W, THETA)`` as above but for an so(3) motion of W*THETA where ``W`` is a
unit-norm vector representing a rotation axis and a rotation magnitude
given by ``THETA``. ``W`` is expressed as a 1-vector (array_like).
For se(2) the results is an SE(2) homogeneous transformation matrix:
- ``trexp2(SIGMA)`` is the matrix exponential of the se(2) element ``SIGMA`` which is
a 3x3 augmented skew-symmetric matrix.
- ``trexp2(SIGMA, THETA)`` as above but for an se(3) motion of SIGMA*THETA, where ``SIGMA``
must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric
matrix.
- ``trexp2(TW)`` is the matrix exponential of the se(3) element ``TW`` represented as
a 3-vector which can be considered a screw motion.
- ``trexp2(TW, THETA)`` as above but for an se(2) motion of TW*THETA, where ``TW``
must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric
matrix.
:seealso: trlog, trexp2
"""
if argcheck.ismatrix(S, (3, 3)) or argcheck.isvector(S, 3):
# se(2) case
if argcheck.ismatrix(S, (3, 3)):
# augmentented skew matrix
tw = trn.vexa(S)
else:
# 3 vector
tw = argcheck.getvector(S)
if theta is None:
(tw, theta) = vec.unittwist2(tw)
else:
assert vec.isunittwist2(
tw), 'If theta is specified S must be a unit twist'
t = tw[0:2]
w = tw[2]
R = trn._rodrigues(w, theta)
skw = trn.skew(w)
V = np.eye(2) * theta + (1.0 - math.cos(theta)) * skw + (
theta - math.sin(theta)) * skw @ skw
return trn.rt2tr(R, V @ t)
elif argcheck.ismatrix(S, (2, 2)) or argcheck.isvector(S, 1):
# so(2) case
if argcheck.ismatrix(S, (2, 2)):
# skew symmetric matrix
w = trn.vex(S)
else:
# 1 vector
w = argcheck.getvector(S)
if theta is not None:
assert vec.isunitvec(
w), 'If theta is specified S must be a unit twist'
# do Rodrigues' formula for rotation
return trn._rodrigues(w, theta)
else:
raise ValueError(
" First argument must be SO(2), 1-vector, SE(2) or 3-vector")
def trinterp2(end, start=None, s=None):
"""
Interpolate SE(2) matrices
:param end: final SE(3) matrix, value when s=1
:type T0: np.ndarray, shape=(3,3)
:param start: initial SE(3) matrix, value when s=0, optional, defaults to identity
:type T1: np.ndarray, shape=(3,3)
:param s: interpolation coefficient, range 0 to 1
:type s: float
:return: SE(2) matrix
:rtype: np.ndarray, shape=(3,3)
- ``trinterp2(T1, s=S)`` is a homogeneous transform (3x3) interpolated
between identity when S=0 and T1 when S=1.
- ``trinterp2(T1, start=T0, s=S)`` as above but interpolated
between T0 when S=0 and T1 when S=1. T0 and T1 are both homogeneous
transforms (3x3).
Notes:
- Rotation angle is linearly interpolated.
:seealso: :func:`~spatialmath.base.transforms3d.trinterp`
%## 2d homogeneous trajectory
"""
if argcheck.ismatrix(start, (2,2)):
# SO(2) case
if T1 is None:
# TRINTERP2(T, s)
th0 = math.atan2(start[1,0], start[0,0])
th = s * th0
else:
# TRINTERP2(T0, T1, s)
assert start.shape == end.shape, 'both matrices must be same shape'
th0 = math.atan2(start[1,0], start[0,0])
th1 = math.atan2(end[1,0], end[0,0])
th = th0 * (1 - s) + s * th1
return rot2(th)
elif argcheck.ismatrix(start, (3,3)):
if end is None:
# TRINTERP2(T, s)
th0 = math.atan2(start[1,0], start[0,0])
p0 = transl2(start)
th = s * th0
pr = s * p0
else:
# TRINTERP2(T0, T1, s)
assert start.shape == end.shape, 'both matrices must be same shape'
th0 = math.atan2(start[1,0], start[0,0])
th1 = math.atan2(end[1,0], end[0,0])
p0 = transl2(start)
p1 = transl2(end)
pr = p0 * (1 - s) + s * p1;
th = th0 * (1 - s) + s * th1
return trn.rt2tr(rot2(th), pr)
else:
return ValueError('Argument must be SO(2) or SE(2)')
def trinterp2(start, end, s=None):
"""
Interpolate SE(2) matrices
:param start: initial SO(2) or SE(2) matrix value when s=0, if None then identity is used
:type T1: np.ndarray, shape=(2,2), (3,3) or None
:param end: final SO(2) or SE(2) matrix, value when s=1
:type T0: np.ndarray, shape=(2,2), (3,3)
:param s: interpolation coefficient, range 0 to 1
:type s: float
:return: SO(2) or SE(2) matrix
:rtype: np.ndarray, shape=(2,2), (3,3)
- ``trinterp2(None, T, S)`` is a homogeneous transform (3x3) interpolated
between identity when S=0 and T (3x3) when S=1.
- ``trinterp2(T0, T1, S)`` as above but interpolated
between T0 (3x3) when S=0 and T1 (3x3) when S=1.
- ``trinterp2(None, R, S)`` is a rotation matrix (2x2) interpolated
between identity when S=0 and R (2x2) when S=1.
- ``trinterp2(R0, R1, S)`` as above but interpolated
between R0 (2x2) when S=0 and R1 (2x2) when S=1.
Notes:
- Rotation angle is linearly interpolated.
:seealso: :func:`~spatialmath.base.transforms3d.trinterp`
%## 2d homogeneous trajectory
"""
if argcheck.ismatrix(end, (2, 2)):
# SO(2) case
if start is None:
# TRINTERP2(T, s)
th0 = math.atan2(end[1, 0], end[0, 0])
th = s * th0
else:
# TRINTERP2(T1, start= s)
assert start.shape == end.shape, 'start and end matrices must be same shape'
th0 = math.atan2(start[1, 0], start[0, 0])
th1 = math.atan2(end[1, 0], end[0, 0])
th = th0 * (1 - s) + s * th1
return rot2(th)
elif argcheck.ismatrix(end, (3, 3)):
if start is None:
# TRINTERP2(T, s)
th0 = math.atan2(end[1, 0], end[0, 0])
p0 = transl2(end)
th = s * th0
pr = s * p0
else:
# TRINTERP2(T0, T1, s)
assert start.shape == end.shape, 'both matrices must be same shape'
th0 = math.atan2(start[1, 0], start[0, 0])
th1 = math.atan2(end[1, 0], end[0, 0])
p0 = transl2(start)
p1 = transl2(end)
pr = p0 * (1 - s) + s * p1
th = th0 * (1 - s) + s * th1
return trn.rt2tr(rot2(th), pr)
else:
return ValueError('Argument must be SO(2) or SE(2)')