def trplot2(self, Tf, T0=None, **kwargs):
"""
Define the transform to animate
:param T: an SO(3) or SE(3) pose to be displayed as coordinate frame
:type: numpy.ndarray, shape=(3,3) or (4,4)
Is polymorphic with ``base.trplot`` and accepts the same parameters.
This sets up the animation but doesn't execute it.
:seealso: :func:`run`
"""
# stash the final value
if tr.isrot2(Tf):
self.Tf = tr.r2t(Tf)
if T0 is None:
self.T0 = np.eye(3)
else:
self.T0 = tr.r2t(T0)
else:
self.Tf = Tf
if T0 is None:
self.T0 = np.eye(3)
else:
self.T0 = T0
# draw axes at the origin
tr.trplot2(np.eye(3), axes=self, **kwargs)
def trplot2(self, end, start=None, **kwargs):
"""
Define the transform to animate
:param end: the final pose SO(2) or SE(2) to display as a coordinate frame
:type end: numpy.ndarray, shape=(2,2) or (3,3)
:param start: the initial pose SO(2) or SE(2) to display as a coordinate frame, defaults to null
:type start: numpy.ndarray, shape=(2,2) or (3,3)
Is polymorphic with ``base.trplot`` and accepts the same parameters.
This sets up the animation but doesn't execute it.
:seealso: :func:`run`
"""
# stash the final value
if tr.isrot2(end):
self.end = tr.r2t(end)
else:
self.end = end
if start is None:
self.start = np.identity(3)
else:
if tr.isrot2(start):
self.start = tr.r2t(start)
else:
self.start = start
# draw axes at the origin
tr.trplot2(self.start, axes=self, block=False, **kwargs)
def Rand(cls, *, xrange=(-1, 1), yrange=(-1, 1), zrange=(-1, 1), N=1):
"""
Create a random SE(3)
: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 zrange: z-axis range [min,max], defaults to [-1, 1]
:type zrange: 2-element sequence, optional
:param N: number of random transforms
:type N: int
:return: homogeneous transformation matrix
:rtype: SE3 instance
Return an SE3 instance with random rotation and translation.
- ``SE3.Rand()`` is a random SE(3) translation.
- ``SE3.Rand(N)`` is an SE3 object containing a sequence of N random
poses.
:seealso: `~spatialmath.quaternion.UnitQuaternion.Rand`
"""
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
Z = np.random.uniform(low=yrange[0], high=zrange[1],
size=N) # random values in the range
R = SO3.Rand(N=N)
return cls([
tr.transl(x, y, z) @ tr.r2t(r.A)
for (x, y, z, r) in zip(X, Y, Z, R)
],
check=False)
def Rand(cls, *, xrange=[-1, 1], yrange=[-1, 1], zrange=[-1, 1], N=1):
"""
Create SE(3) with pure random rotation
:param N: number of random rotations
:type N: int
:return: 4x4 homogeneous transformation matrix
:rtype: SE3 instance
- ``SE3.Rand()`` is a random SO(3) rotation.
- ``SE3.Rand(N)`` is an SO3 object containing a sequence of N random
rotations.
:seealso: `~spatialmath.quaternion.UnitQuaternion.Rand`
"""
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
Z = np.random.uniform(low=yrange[0], high=zrange[1],
size=N) # random values in the range
R = SO3.Rand(N=N)
return cls([
tr.transl(x, y, z) @ tr.r2t(r.A)
for (x, y, z, r) in zip(X, Y, Z, R)
])
def SO3(cls, R, check=True):
if isinstance(R, SO3):
R = R.A
elif base.isrot(R, check=check):
pass
else:
raise ValueError('expecting SO3 or rotation matrix')
return cls(base.r2t(R))
def __init__(self, x=None, y=None, z=None, *, check=True):
"""
Construct new SE(3) object
:rtype: SE3 instance
There are multiple call signatures that return an ``SE3`` instance
with one or more values.
- ``SE3()`` null motion, value is the identity matrix.
- ``SE3(x, y, z)`` is a pure translation of (x,y,z)
- ``SE3(T)`` where ``T`` is a 4x4 Numpy array representing an SE(3)
matrix. If ``check`` is ``True`` check the matrix belongs to SE(3).
- ``SE3([T1, T2, ... TN])`` has ``N`` values
given by the elements ``Ti`` each of which is a 4x4 NumPy array
representing an SE(3) matrix. If ``check`` is ``True`` check the
matrix belongs to SE(3).
- ``SE3(X)`` where ``X`` is:
- ``SE3`` is a copy of ``X``
- ``SO3`` is the rotation of ``X`` with zero translation
- ``SE2`` is the z-axis rotation and x- and y-axis translation of
``X``
- ``SE3([X1, X2, ... XN])`` has ``N`` values
given by the elements ``Xi`` each of which is an SE3 instance.
:SymPy: supported
"""
if y is None and z is None:
# just one argument passed
if super().arghandler(x, check=check):
return
elif isinstance(x, SO3):
self.data = [base.r2t(_x) for _x in x.data]
elif type(x).__name__ == 'SE2':
def convert(x):
# convert SE(2) to SE(3)
out = np.identity(4, dtype=x.dtype)
out[:2, :2] = x[:2, :2]
out[:2, 3] = x[:2, 2]
return out
self.data = [convert(_x) for _x in x.data]
elif base.isvector(x, 3):
# SE3( [x, y, z] )
self.data = [base.transl(x)]
elif isinstance(x, np.ndarray) and x.shape[1] == 3:
# SE3( Nx3 )
self.data = [base.transl(T) for T in x]
else:
raise ValueError('bad argument to constructor')
elif y is not None and z is not None:
# SE3(x, y, z)
self.data = [base.transl(x, y, z)]
def trplot(self, end, start=None, **kwargs):
"""
Define the transform to animate
:param end: the final pose SE(3) or SO(3) to display as a coordinate frame
:type end: ndarray(4,4) or ndarray(3,3)
:param start: the initial pose SE(3) or SO(3) to display as a coordinate frame, defaults to null
:type start: ndarray(4,4) or ndarray(3,3)
:param start: an
Is polymorphic with ``base.trplot`` and accepts the same parameters.
This sets up the animation but doesn't execute it.
:seealso: :func:`run`
"""
if not isinstance(end, (np.ndarray, np.generic)) and isinstance(
end, Iterable):
try:
if len(end) == 1:
end = end[0]
elif len(end) >= 2:
self.trajectory = end
except TypeError:
# a generator has no len()
self.trajectory = end
# stash the final value
if base.isrot(end):
self.end = base.r2t(end)
else:
self.end = end
if start is None:
self.start = np.identity(4)
else:
if base.isrot(start):
self.start = base.r2t(start)
else:
self.start = start
# draw axes at the origin
base.trplot(self.start, axes=self, **kwargs)
def Rand(cls, *, xrange=(-1, 1), yrange=(-1, 1), zrange=(-1, 1), N=1): # pylint: disable=arguments-differ
"""
Create a random SE(3)
: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 zrange: z-axis range [min,max], defaults to [-1, 1]
:type zrange: 2-element sequence, optional
:param N: number of random transforms
:type N: int
:return: SE(3) matrix
:rtype: SE3 instance
Return an SE3 instance with random rotation and translation.
- ``SE3.Rand()`` is a random SE(3) translation.
- ``SE3.Rand(N=N)`` is an SE3 object containing a sequence of N random
poses.
Example::
>>> SE3.Rand(N=2)
SE3([
array([[ 0.58076657, 0.64578702, -0.49565041, -0.78585825],
[-0.57373134, -0.10724881, -0.8119914 , 0.72069253],
[-0.57753142, 0.75594763, 0.30822173, 0.12291999],
[ 0. , 0. , 0. , 1. ]]),
array([[ 0.96481299, -0.26267256, -0.01179066, 0.80294729],
[ 0.06421463, 0.19190584, 0.97931028, -0.15021311],
[-0.25497525, -0.94560841, 0.20202067, 0.02684599],
[ 0. , 0. , 0. , 1. ]]) ])
:seealso: :func:`~spatialmath.quaternions.UnitQuaternion.Rand`
"""
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
Z = np.random.uniform(low=yrange[0], high=zrange[1],
size=N) # random values in the range
R = SO3.Rand(N=N)
return cls([
base.transl(x, y, z) @ base.r2t(r.A)
for (x, y, z, r) in zip(X, Y, Z, R)
],
check=False)
def update(frame, animation):
# frame is the result of calling next() on a iterator or generator
# seemingly the animation framework isn't checking StopException
# so there is no way to know when this is no longer called.
# we implement a rather hacky heartbeat style timeout
if isinstance(frame, float):
# passed a single transform, interpolate it
T = base.trinterp(start=self.start, end=self.end, s=frame)
else:
# assume it is an SO(3) or SE(3)
T = frame
# ensure result is SE(3)
if T.shape == (3, 3):
T = base.r2t(T)
# update the scene
animation._draw(T)
self.count += 1 # say we're still running
return animation.artists()
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
if isinstance(x, SO2):
self.data = [base.r2t(_x) for _x in x.data]
elif argcheck.isscalar(x):
self.data = [base.trot2(x, unit=unit)]
elif len(x) == 2:
# SE2([x,y])
self.data = [base.transl2(x)]
elif len(x) == 3:
# SE2([x,y,theta])
self.data = [base.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 = [base.transl2(x, y)]
elif y is not None and theta is not None:
# SE2(x, y, theta)
self.data = [base.trot2(theta, t=[x, y], unit=unit)]
else:
raise ValueError('bad arguments to constructor')
def SE3(self):
return p3d.SE3(tr.r2t(self.R), check=False)
def trplot2(
T,
axes=None,
block=True,
dims=None,
color='blue',
frame=None, # pylint: disable=unused-argument,function-redefined
textcolor=None,
labels=('X', 'Y'),
length=1,
arrow=True,
rviz=False,
wtl=0.2,
width=1,
d1=0.05,
d2=1.15,
**kwargs):
"""
Plot a 2D coordinate frame
:param T: an SE(3) or SO(3) pose to be displayed as coordinate frame
:type: ndarray(3,3) or ndarray(2,2)
:param axes: the axes to plot into, defaults to current axes
:type axes: Axes3D reference
:param block: run the GUI main loop until all windows are closed, default True
:type block: bool
:param dims: dimension of plot volume as [xmin, xmax, ymin, ymax]
:type dims: array_like(4)
:param color: color of the lines defining the frame
:type color: str
:param textcolor: color of text labels for the frame, default color of lines above
:type textcolor: str
:param frame: label the frame, name is shown below the frame and as subscripts on the frame axis labels
:type frame: str
:param labels: labels for the axes, defaults to X, Y and Z
:type labels: 3-tuple of strings
:param length: length of coordinate frame axes, default 1
:type length: float
:param arrow: show arrow heads, default True
:type arrow: bool
:param wtl: width-to-length ratio for arrows, default 0.2
:type wtl: float
:param rviz: show Rviz style arrows, default False
:type rviz: bool
:param projection: 3D projection: ortho [default] or persp
:type projection: str
:param width: width of lines, default 1
:type width: float
:param d1: distance of frame axis label text from origin, default 1.15
:type d2: distance of frame label text from origin, default 0.05
:return: axes containing the frame
:rtype: AxesSubplot
:raises ValueError: bad argument
Adds a 2D coordinate frame represented by the SO(2) or SE(2) matrix to the current axes.
- If no current figure, one is created
- If current figure, but no axes, a 3d Axes is created
Examples:
trplot2(T, frame='A')
trplot2(T, frame='A', color='green')
trplot2(T1, 'labels', 'AB');
"""
# TODO
# animation
# style='line', 'arrow', 'rviz'
# check input types
if isrot2(T, check=True):
T = base.r2t(T)
elif not ishom2(T, check=True):
raise ValueError("argument is not valid SE(2) matrix")
if axes is None:
# create an axes
fig = plt.gcf()
if fig.axes == []:
# no axes in the figure, create a 3D axes
ax = plt.gca()
if dims is None:
ax.autoscale(enable=True, axis='both')
else:
if len(dims) == 2:
dims = dims * 2
ax.set_xlim(dims[0:2])
ax.set_ylim(dims[2:4])
ax.set_aspect('equal')
ax.set_xlabel(labels[0])
ax.set_ylabel(labels[1])
else:
# reuse an existing axis
ax = plt.gca()
else:
ax = axes
# create unit vectors in homogeneous form
o = T @ np.array([0, 0, 1])
x = T @ np.array([1, 0, 1]) * length
y = T @ np.array([0, 1, 1]) * length
# draw the axes
if rviz:
ax.plot([o[0], x[0]], [o[1], x[1]],
color='red',
linewidth=5 * width)
ax.plot([o[0], y[0]], [o[1], y[1]],
color='lime',
linewidth=5 * width)
elif arrow:
ax.quiver(o[0],
o[1],
x[0] - o[0],
x[1] - o[1],
angles='xy',
scale_units='xy',
scale=1,
linewidth=width,
facecolor=color,
edgecolor=color)
ax.quiver(o[0],
o[1],
y[0] - o[0],
y[1] - o[1],
angles='xy',
scale_units='xy',
scale=1,
linewidth=width,
facecolor=color,
edgecolor=color)
# plot an invisible point at the end of each arrow to allow auto-scaling to work
ax.scatter(x=[o[0], x[0], y[0]],
y=[o[1], x[1], y[1]],
s=[20, 0, 0])
else:
ax.plot([o[0], x[0]], [o[1], x[1]], color=color, linewidth=width)
ax.plot([o[0], y[0]], [o[1], y[1]], color=color, linewidth=width)
# label the frame
if frame:
if textcolor is not None:
color = textcolor
o1 = T @ np.array([-d1, -d1, 1])
ax.text(o1[0],
o1[1],
r'$\{' + frame + r'\}$',
color=color,
verticalalignment='top',
horizontalalignment='center')
# add the labels to each axis
x = (x - o) * d2 + o
y = (y - o) * d2 + o
ax.text(x[0],
x[1],
"$%c_{%s}$" % (labels[0], frame),
color=color,
horizontalalignment='center',
verticalalignment='center')
ax.text(y[0],
y[1],
"$%c_{%s}$" % (labels[1], frame),
color=color,
horizontalalignment='center',
verticalalignment='center')
if block:
# calling this at all, causes FuncAnimation to fail so when invoked from tranimate2 skip this bit
plt.show(block=block)
return ax
def _twist(x, y, z, r):
T = base.transl(x, y, z) @ base.r2t(r.A)
return base.trlog(T, twist=True)
