def eul2r(phi, theta=None, psi=None, unit='rad'):
"""
Create an SO(3) rotation matrix from Euler angles
:param phi: Z-axis rotation
:type phi: float
:param theta: Y-axis rotation
:type theta: float
:param psi: Z-axis rotation
:type psi: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: 3x3 rotation matrix
:rtype: numpdy.ndarray, shape=(3,3)
- ``R = eul2r(PHI, THETA, PSI)`` is an SO(3) orthonornal rotation
matrix equivalent to the specified Euler angles. These correspond
to rotations about the Z, Y, Z axes respectively.
- ``R = eul2r(EUL)`` as above but the Euler angles are taken from
``EUL`` which is a 3-vector (array_like) with values
(PHI THETA PSI).
:seealso: :func:`~rpy2r`, :func:`~eul2tr`, :func:`~tr2eul`
"""
if np.isscalar(phi):
angles = [phi, theta, psi]
else:
angles = argcheck.getvector(phi, 3)
angles = argcheck.getunit(angles, unit)
return rotz(angles[0]) @ roty(angles[1]) @ rotz(angles[2])
def Rand(cls, N=1, xrange=(-1, 1), yrange=(-1, 1), arange=(0, 2 * math.pi), unit='rad'): # 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([base.trot2(t, t=[x, y]) for (t, x, y) in zip(x, y, argcheck.getunit(theta, unit))])
def angvec(cls, theta, v, unit='rad'):
v = argcheck.getvector(v, 3)
argcheck.isscalar(theta)
theta = argcheck.getunit(theta, unit)
return UnitQuaternion(s=math.cos(theta / 2),
v=math.sin(theta / 2) * tr.unit(v),
norm=False)
def exp(self, theta=None, units='rad'):
"""
Twist.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 twist.
Notes::
- For the second form the twist must, if rotational, have a unit rotational component.
See also Twist.T, trexp, trexp2.
"""
if units != 'rad' and self.isprismatic:
print('Twist.exp: using degree mode for a prismatic twist')
if theta is None:
theta = 1
else:
theta = argcheck.getunit(theta, units)
if isinstance(theta, (int, np.int64, float, np.float64)):
return SE2(tr.trexp2(self.S * theta))
else:
return SE2([tr.trexp2(self.S * t) for t in theta])
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 twist.
Notes::
- For the second form the twist must, if rotational, have a unit rotational component.
See also Twist.T, trexp, trexp2.
"""
if units != 'rad' and self.isprismatic:
print('Twist.exp: using degree mode for a prismatic twist')
if theta is None:
theta = 1
else:
theta = argcheck.getunit(theta, units)
if isinstance(theta, (int, np.int64, float, np.float64)):
return SE3(tr.trexp(self.S * theta))
else:
return SE3([tr.trexp(self.S * t) for t in theta])
def rpy2r(roll, pitch=None, yaw=None, *, unit='rad', order='zyx'):
"""
Create an SO(3) rotation matrix from roll-pitch-yaw angles
:param roll: roll angle
:type roll: float
:param pitch: pitch angle
:type pitch: float
:param yaw: yaw angle
:type yaw: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param unit: rotation order: 'zyx' [default], 'xyz', or 'yxz'
:type unit: str
:return: 3x3 rotation matrix
:rtype: numpdy.ndarray, shape=(3,3)
- ``rpy2r(ROLL, PITCH, YAW)`` is an SO(3) orthonormal rotation matrix
(3x3) equivalent to the specified roll, pitch, yaw angles angles.
These correspond to successive rotations about the axes specified by ``order``:
- 'zyx' [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
then by roll about the new x-axis. Convention for a mobile robot with x-axis forward
and y-axis sideways.
- 'xyz', rotate by yaw about the x-axis, then by pitch about the new y-axis,
then by roll about the new z-axis. Covention for a robot gripper with z-axis forward
and y-axis between the gripper fingers.
- 'yxz', rotate by yaw about the y-axis, then by pitch about the new x-axis,
then by roll about the new z-axis. Convention for a camera with z-axis parallel
to the optic axis and x-axis parallel to the pixel rows.
- ``rpy2r(RPY)`` as above but the roll, pitch, yaw angles are taken
from ``RPY`` which is a 3-vector (array_like) with values
(ROLL, PITCH, YAW).
:seealso: :func:`~eul2r`, :func:`~rpy2tr`, :func:`~tr2rpy`
"""
if np.isscalar(roll):
angles = [roll, pitch, yaw]
else:
angles = argcheck.getvector(roll, 3)
angles = argcheck.getunit(angles, unit)
if order == 'xyz' or order == 'arm':
R = rotx(angles[2]) @ roty(angles[1]) @ rotz(angles[0])
elif order == 'zyx' or order == 'vehicle':
R = rotz(angles[2]) @ roty(angles[1]) @ rotx(angles[0])
elif order == 'yxz' or order == 'camera':
R = roty(angles[2]) @ rotx(angles[1]) @ rotz(angles[0])
else:
raise ValueError('Invalid angle order')
return R
def rand(cls,
*,
xrange=[-1, 1],
yrange=[-1, 1],
trange=[0, 2 * math.pi],
unit='rad',
N=1):
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=trange[0], high=trange[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))
])
def Rand(cls,
*,
xrange=[-1, 1],
yrange=[-1, 1],
trange=[0, 2 * math.pi],
unit='rad',
N=1):
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 trange: theta range [min,max], defaults to :math:`[0, 2\pi)`
:type yrange: 2-element sequence, optional
:param N: number of random rotations, defaults to 1
:type N: int
: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=trange[0], high=trange[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))
])
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: 2x2 rotation matrix
:rtype: numpy.ndarray, shape=(2,2)
- ``ROT2(THETA)`` is an SO(2) rotation matrix (2x2) representing a rotation of THETA radians.
- ``ROT2(THETA, 'deg')`` as above but THETA is in degrees.
"""
theta = argcheck.getunit(theta, unit)
ct = sym.cos(theta)
st = sym.sin(theta)
R = np.array([[ct, -st], [st, ct]])
return R
def rotz(theta, unit="rad"):
"""
Create SO(3) rotation about Z-axis
:param theta: rotation angle about Z-axis
:type theta: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: 3x3 rotation matrix
:rtype: numpy.ndarray, shape=(3,3)
- ``rotz(THETA)`` is an SO(3) rotation matrix (3x3) representing a rotation
of THETA radians about the z-axis
- ``rotz(THETA, "deg")`` as above but THETA is in degrees
:seealso: :func:`~yrotz`
"""
theta = argcheck.getunit(theta, unit)
ct = _cos(theta)
st = _sin(theta)
R = np.array([[ct, -st, 0], [st, ct, 0], [0, 0, 1]])
return R
def Rand(cls, N=1, arange=(0, 2 * math.pi), unit='rad'):
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([base.rot2(x) for x in argcheck.getunit(rand, unit)])
def addconfiguration(self, name, q, unit='rad'):
"""
Add a named joint configuration (Robot superclass)
:param name: Name of the joint configuration
:type name: str
:param q: Joint configuration
:type q: ndarray(n) or list
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> robot = rtb.models.DH.Puma560()
>>> robot.qz
>>> robot.addconfiguration("mypos", [0.1, 0.2, 0.3, 0.4, 0.5, 0.6])
>>> robot.mypos
"""
v = getvector(q, self.n)
v = getunit(v, unit)
self._configdict[name] = v
setattr(self, name, v)
def angvec2r(theta, v, unit='rad'):
"""
Create an SO(3) rotation matrix from rotation angle and axis
:param theta: rotation
:type theta: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param v: rotation axis, 3-vector
:type v: array_like
:return: 3x3 rotation matrix
:rtype: numpdy.ndarray, shape=(3,3)
``angvec2r(THETA, V)`` is an SO(3) orthonormal rotation matrix
equivalent to a rotation of ``THETA`` about the vector ``V``.
Notes:
- If ``THETA == 0`` then return identity matrix.
- If ``THETA ~= 0`` then ``V`` must have a finite length.
:seealso: :func:`~angvec2tr`, :func:`~tr2angvec`
"""
assert np.isscalar(theta) and argcheck.isvector(
v, 3), "Arguments must be theta and vector"
if np.linalg.norm(v) < 10 * _eps:
return np.eye(3)
theta = argcheck.getunit(theta, unit)
# Rodrigue's equation
sk = trn.skew(vec.unitvec(v))
R = np.eye(3) + math.sin(theta) * sk + (1.0 - math.cos(theta)) * sk @ sk
return R
def __init__(self, x = None, y = None, theta = None, *, unit='rad'):
super().__init__() # activate the UserList semantics
if x is None:
# empty constructor
self.data = [np.eye(3)]
elif all(map(lambda x: isinstance(x, (int,float)), [x, y, theta])):
# SE2(x, y, theta)
self.data = [transforms.trot2(theta, t=[x,y], unit=unit)]
elif argcheck.isvector(x) and argcheck.isvector(y) and argcheck.isvector(theta):
# SE2(xvec, yvec, tvec)
xvec = argcheck.getvector(x)
yvec = argcheck.getvector(y, dim=len(xvec))
tvec = argcheck.getvector(theta, dim=len(xvec))
self.data = [transforms.trot2(_t, t=[_x, _y]) for (_x, _y, _t) in zip(xvec, yvec, argcheck.getunit(tvec, unit))]
elif isinstance(x, np.ndarray) and y is None and theta is None:
assert x.shape[1] == 3, 'array argument must be Nx3'
self.data = [transforms.trot2(_t, t=[_x, _y], unit=unit) for (_x, _y, _t) in x]
else:
super().arghandler(x)
def rand(cls, *, range=[0, 2*math.pi], unit='rad', N=1):
rand = np.random.uniform(low=range[0], high=range[1], size=N) # random values in the range
return cls([transforms.rot2(x) for x in argcheck.getunit(rand, unit)])
