def exp(self, theta=None, units='rad'):
"""
Exponentiate a twist
:param theta: DESCRIPTION, defaults to None
:type theta: TYPE, optional
:param units: DESCRIPTION, defaults to 'rad'
:type units: TYPE, optional
:return: DESCRIPTION
:rtype: TYPE
TW.exp is the homogeneous transformation equivalent to the twist (SE2 or SE3).
TW.exp(THETA) as above but with a rotation of THETA about the Twist3.
Notes::
- For the second form the twist must, if rotational, have a unit rotational component.
See also Twist3.T, trexp, trexp2.
"""
if units != 'rad' and self.isprismatic:
print('Twist3.exp: using degree mode for a prismatic twist')
if theta is None:
theta = 1
else:
theta = base.getunit(theta, units)
if base.isscalar(theta):
return SE3(base.trexp(self.S * theta))
else:
return SE3([base.trexp(self.S * t) for t in theta])
def exp(self, theta=None, units='rad'):
"""
Twist3.exp Convert twist to homogeneous transformation
TW.exp is the homogeneous transformation equivalent to the twist (SE2 or SE3).
TW.exp(THETA) as above but with a rotation of THETA about the Twist3.
Notes::
- For the second form the twist must, if rotational, have a unit rotational component.
See also Twist3.T, trexp, trexp2.
"""
if units != 'rad' and self.isprismatic:
print('Twist3.exp: using degree mode for a prismatic twist')
if theta is None:
theta = 1
else:
theta = base.getunit(theta, units)
if base.isscalar(theta):
return SE2(base.trexp2(self.S * theta))
else:
return SE2([base.trexp2(self.S * t) for t in theta])
def rot2(theta, unit='rad'):
"""
Create SO(2) rotation
:param theta: rotation angle
:type theta: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: SO(2) rotation matrix
:rtype: ndarray(2,2)
- ``rot2(θ)`` is an SO(2) rotation matrix (2x2) representing a rotation of θ radians.
- ``rot2(θ, 'deg')`` as above but θ is in degrees.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> rot2(0.3)
>>> rot2(45, 'deg')
"""
theta = base.getunit(theta, unit)
ct = base.sym.cos(theta)
st = base.sym.sin(theta)
R = np.array([[ct, -st], [st, ct]])
return R
def Rz(cls, theta, unit='rad', t=None):
"""
Create a new 3D twist for pure rotation about the Z-axis
:param θ: rotation angle about Z-axis
:type θ: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: 3D twist vector
:rtype: Twist3 instance
- ``Twist3.Rz(θ)`` is an SO(3) rotation of θ radians about the z-axis
- ``Twist3.Rz(θ, "deg")`` as above but θ is in degrees
If ``θ`` is an array then the result is a sequence of rotations defined
by consecutive elements.
Example:
.. runblock:: pycon
>>> from spatialmath import Twist3
>>> Twist3.Rz(0.3)
>>> Twist3.Rz([0.3, 0.4])
:seealso: :func:`~spatialmath.base.transforms3d.trotz`
:SymPy: supported
"""
return cls(
[np.r_[0, 0, 0, 0, 0, x] for x in base.getunit(theta, unit=unit)])
def exp(self, theta=None, units='rad'):
"""
Exponentiate a 3D twist
:param theta: rotation magnitude, defaults to None
:type theta: float, optional
:param units: rotational units, defaults to 'rad'
:type units: str, optional
:return: SE(3) matrix
:rtype: SE3 instance
- ``X.exp()`` is the homogeneous transformation equivalent to the twist,
:math:`e^{[S]}`
- ``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis,
:math:`e^{\theta[S]}`
Example:
.. runblock:: pycon
>>> from spatialmath import SE3, Twist3
>>> T = SE3(1, 2, 3) * SE3.Rx(0.3)
>>> S = Twist3(T)
>>> S.exp(0)
>>> S.exp(1)
.. notes::
- For the second form, the twist must, if rotational, have a unit
rotational component.
:seealso: :func:`spatialmath.base.trexp`
"""
if units != 'rad' and self.isprismatic:
print('Twist3.exp: using degree mode for a prismatic twist')
if theta is None:
theta = 1
else:
theta = base.getunit(theta, units)
if base.isscalar(theta):
# theta is a scalar
return SE3(base.trexp(self.S * theta))
else:
# theta is a vector
if len(self) == 1:
return SE3([base.trexp(self.S * t) for t in theta])
elif len(self) == len(theta):
return SE3(
[base.trexp(S * t) for S, t in zip(self.data, theta)])
else:
raise ValueError('length of twist and theta not consistent')
def exp(self, theta=None, units='rad'):
r"""
Exponentiate a 2D twist
:param theta: rotation magnitude, defaults to None
:type theta: float, optional
:param units: rotational units, defaults to 'rad'
:type units: str, optional
:return: SE(2) matrix
:rtype: SE2 instance
- ``X.exp()`` is the homogeneous transformation equivalent to the twist,
:math:`e^{[S]}`
- ``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis,
:math:`e^{\theta[S]}`
Example:
.. runblock:: pycon
>>> from spatialmath import SE2, Twist2
>>> T = SE2(1, 2, 0.3)
>>> S = Twist2(T)
>>> S.exp(0)
>>> S.exp(1)
.. notes::
- For the second form, the twist must, if rotational, have a unit
rotational component.
:seealso: :func:`spatialmath.base.trexp2`
"""
if units != 'rad' and self.isprismatic:
print('Twist3.exp: using degree mode for a prismatic twist')
if theta is None:
theta = 1
else:
theta = base.getunit(theta, units)
if base.isscalar(theta):
return SE2(base.trexp2(self.S * theta))
else:
return SE2([base.trexp2(self.S * t) for t in theta])
def exp(self, theta=None, units='rad'):
"""
Exponentiate a 3D twist
:param theta: DESCRIPTION, defaults to None
:type theta: TYPE, optional
:param units: DESCRIPTION, defaults to 'rad'
:type units: TYPE, optional
:return: homogeneous transformation
:rtype: SE3
-``X.exp()`` is the homogeneous transformation equivalent to the twist.
-``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis.
.. notes::
- For the second form, the twist must, if rotational, have a unit
rotational component.
See also Twist3.T, trexp, trexp2.
"""
if units != 'rad' and self.isprismatic:
print('Twist3.exp: using degree mode for a prismatic twist')
if theta is None:
theta = 1
else:
theta = base.getunit(theta, units)
if base.isscalar(theta):
# theta is a scalar
return SE3(base.trexp(self.S * theta))
else:
# theta is a vector
if len(self) == 1:
return SE3([base.trexp(self.S * t) for t in theta])
elif len(self) == len(theta):
return SE3(
[base.trexp(S * t) for S, t in zip(self.data, theta)])
else:
raise ValueError('length of twist and theta not consistent')
def __init__(self,
axis_func=None,
axis=None,
eta=None,
unit='rad',
j=None,
flip=False):
super().__init__() # init UserList superclass
if axis_func is None and axis is None and eta is None:
# ET()
# create instance with no values
self.data = []
return
elif isinstance(axis_func, ETS):
# copy constructor
e = axis_func
axis_func = e.axis_func
axis = e.axis
# et = e.eta
j = e.jindex
flip = e.isflip
joint = e.isjoint
T = e.T
elif callable(axis_func):
if eta is None:
# no value, it's a variable joint
if unit != 'rad':
raise ValueError(
'can only use radians for a variable transform')
joint = True
T = None
else:
# constant value specified
joint = False
eta = getunit(eta, unit)
T = axis_func(eta)
if j is not None:
raise ValueError(
'cannot specify joint index for a constant ET')
if flip:
raise ValueError('cannot specify flip for a constant ET')
elif axis == 'C':
# it's a constant element Ci
if isinstance(self, ETS):
# ETS
if not isinstance(eta, np.ndarray):
T = eta.A
else:
T = eta
if T.shape != (4, 4):
raise ValueError('argument must be ndarray(4,4) or SE3')
else:
# ETS2
if not isinstance(eta, np.ndarray):
T = eta.A
else:
T = eta
if T.shape != (3, 3):
raise ValueError('argument must be ndarray(3,3) or SE2')
axis = "C"
joint = False
axis_func = None
else:
raise ValueError('axis_func must be callable or ndarray')
# Save all the params in a named tuple
e = SimpleNamespace(eta=eta,
axis_func=axis_func,
axis=axis,
joint=joint,
T=T,
jindex=j,
flip=flip)
# And make it the only value of this instance
self.data = [e]
def __init__(self,
axis=None,
eta=None,
axis_func=None,
unit='rad',
j=None,
flip=False,
qlim=None):
"""
Elementary transform sequence (superclass)
:param axis: the axis. For 2D case: 'r', 'tx', 'ty'.
For 3D case: 'rx', 'ry', 'rz', 'tx', 'ty', 'tz'.
:type axis: str
:param eta: the constant associated with this transform,
not given for a joint transform
:type eta: float or symbol, optional
:param axis_func: [description], defaults to None
:type axis_func: [type], optional
:param unit: unit for ``eta``, 'rad' [default] or 'deg'
:type unit: str
:param j: joint number, for joint transforms only
:type j: int, optional
:param flip: flip the sign of joint variable, defaults to False
:type flip: bool, optional
Examples:
.. runblock:: pycon
>>> from roboticstoolbox import ETS, ETS2
ETS2.r() # variable 2D rotation
ETS2.r(90, unit='deg') # 2D constant rotation
ETS.rx() # variable 3D rotation about x-axis
ETS.tx(1) # constant 3D translation along x-axis
Composition
-----------
These transforms can be composed, for example:
.. runblock:: pycon
>>> from roboticstoolbox import ETS
>>> e = ETS.rx() * ETS.tx(1) * ETS.rx() * ETS.tx(1)
>>> print(e)
>>> len(e)
>>> e[0]
>>> e[1]
Under the hood
--------------
The value of an ETS is obtained using its ``T`` method.
For a joint ETS the joint variable must be passed ``e.T(q)``,
otherwise the value based on ``eta`` is computed and cached
at constructor time.
"""
super().__init__() # init UserList superclass
if axis is None and eta is None and axis_func is None:
# ET()
# create instance with no values
self.data = []
return
elif isinstance(axis, ETS):
# copy constructor
# e = axis_func
# axis_func = e.axis_func
# axis = e.axis
# # et = e.eta
# j = e.jindex
# flip = e.isflip
# joint = e.isjoint
# T = e.T
self.data = copy.copy(axis.data)
return
if axis in ('R', 'Rx', 'Ry', 'Rz', 'tx', 'ty', 'tz'):
# it's a regular axis
if eta is None:
# no value, it's a variable joint
if unit != 'rad':
raise ValueError(
'can only use radians for a variable transform')
joint = True
T = None
else:
# constant value specified
if not callable(axis_func):
raise ValueError('axis func must be callable')
joint = False
eta = getunit(eta, unit)
T = axis_func(eta)
if j is not None:
raise ValueError(
'cannot specify joint index for a constant ET')
if flip:
raise ValueError('cannot specify flip for a constant ET')
elif axis == 'C':
# it's a constant element Ci
if isinstance(self, ETS):
# ETS
if not isinstance(eta, np.ndarray):
T = eta.A
else:
T = eta
if T.shape != (4, 4):
raise ValueError('argument must be ndarray(4,4) or SE3')
else:
# ETS2
if not isinstance(eta, np.ndarray):
T = eta.A
else:
T = eta
if T.shape != (3, 3):
raise ValueError('argument must be ndarray(3,3) or SE2')
axis = "C"
joint = False
axis_func = None
else:
raise ValueError('bad axis specified')
# Save all the params in a named tuple
e = SimpleNamespace(eta=eta,
axis_func=axis_func,
axis=axis,
joint=joint,
T=T,
jindex=j,
flip=flip,
qlim=qlim)
# And make it the only value of this instance
self.data = [e]
