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)