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(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, theta)`` is an SE2 instance representing a translation of (``x``, ``y``) and a rotation of ``theta`` radians - ``SE2(x, y, theta, unit='deg')`` is an SE2 instance representing a translation of (``x``, ``y``) and a rotation of ``theta`` degrees - ``SE2(t)`` is an SE2 instance representing a translation of (``x``, ``y``) and a rotation of ``theta`` where ``t``=[x,y,theta] is a 3-element array_like - ``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. """ super().__init__() # activate the UserList semantics if x is None and y is None and theta is None: # SE2() # empty constructor self.data = [np.eye(3)] elif x is not None: if y is not None and theta is None: # SE2(x, y) self.data = [tr.transl2(x, y)] elif y is not None and theta is not None: # SE2(x, y, theta) self.data = [tr.trot2(theta, t=[x, y], unit=unit)] elif y is None and theta is None: if argcheck.isvector(x, 2): # SE2([x,y]) self.data = [tr.transl2(x)] elif argcheck.isvector(x, 3): # SE2([x,y,theta]) self.data = [tr.trot2(x[2], t=x[:2], unit=unit)] else: super()._arghandler(x, check=check) else: raise ValueError('bad arguments to constructor')
def ty(cls, eta=None, **kwargs): """ Pure translation along the y-axis :param η: translation distance along the y-axis :type η: float :param j: Explicit joint number within the robot :type j: int, optional :param flip: Joint moves in opposite direction :type flip: bool :return: An elementary transform :rtype: ETS instance - ``ETS.tx(η)`` is an elementary translation along the y-axis by a distance constant η - ``ETS.tx()`` is an elementary translation along the y-axis by a variable distance, i.e. a prismatic robot joint. ``j`` or ``flip`` can be set in this case. :seealso: :func:`ETS` """ return cls(axis='ty', eta=eta, axis_func=lambda y: transl2(0, y), **kwargs)
def linear_factors(self, edge): # extract the ids of the vertices connected by the kth edge # id_i = eids(1,k) # id_j = eids(2,k) # extract the poses of the vertices and the mean of the edge # v_i = vmeans(:,id_i) # v_j = vmeans(:,id_j) # z_ij = emeans(:,k) v = g.vertices(edge) v_i = g.coord(v(1)) v_j = g.coord(v(2)) z_ij = g.edata(edge).mean # compute the homoeneous transforms of the previous solutions zt_ij = base.trot2(z_ij[2], t=z_ij[0:2]) vt_i = base.trot2(v_i[2], t=v_i[0:2]) vt_j = base.trot2(v_j[2], t=v_j[0:2]) # compute the displacement between x_i and x_j f_ij = base.trinv2(vt_i @ vt_j) # this below is too long to explain, to understand it derive it by hand theta_i = v_i[3] ti = v_i[0:2, 0] tj = v_j[0:2, 0] dt_ij = tj - ti si = sin(theta_i) ci = cos(theta_i) A = np.array([ [-ci, -si, [-si, ci] @ dt_ij], [si, -ci, [-ci, -si] @ dt_ij], [0, 0, -1], ]) B = np.array([ [ci, si, 0], [-si, ci, 0], [0, 0, 1], ]) ztinv = base.trinv2(zt_ij) T = ztinv @ f_ij e = np.r_[base.transl2(T), base.angle(T)] ztinv[0:2, 2] = 0 A = ztinv @ A B = ztinv @ B return e, A, B
def vexa(Omega, check=False): r""" Convert skew-symmetric matrix to vector :param s: augmented skew-symmetric matrix :type s: ndarray(3,3) or ndarray(4,4) :param check: check if matrix is skew symmetric part is valid (default False, no check) :type check: bool :return: vector of unique values :rtype: ndarray(3) or ndarray(6) :raises ValueError: bad argument ``vexa(S)`` is the vector which has the corresponding augmented skew-symmetric matrix ``S``. - ``S`` is 3x3 - se(2) case - where ``S`` :math:`= \left[ \begin{array}{ccc} 0 & -v_3 & v_1 \\ v_3 & 0 & v_2 \\ 0 & 0 & 0 \end{array} \right]` then return :math:`[v_1, v_2, v_3]`. - ``S`` is 4x4 - se(3) case - where ``S`` :math:`= \left[ \begin{array}{cccc} 0 & -v_6 & v_5 & v_1 \\ v_6 & 0 & -v_4 & v_2 \\ -v_5 & v_4 & 0 & v_3 \\ 0 & 0 & 0 & 0 \end{array} \right]` then return :math:`[v_1, v_2, v_3, v_4, v_5, v_6]`. .. runblock:: pycon >>> from spatialmath.base import * >>> S = skewa([1, 2, 3]) >>> print(S) >>> vexa(S) >>> S = skewa([1, 2, 3, 4, 5, 6]) >>> print(S) >>> vexa(S) .. note:: - This is the inverse of the function ``skewa``. - Only rudimentary checking (zero diagonal) is done to ensure that the matrix is actually skew-symmetric. - The function takes the mean of the two elements that correspond to each unique element of the matrix. :seealso: :func:`skewa`, :func:`vex` :SymPy: supported """ if Omega.shape == (4, 4): return np.hstack((base.transl(Omega), vex(t2r(Omega), check=check))) elif Omega.shape == (3, 3): return np.hstack((base.transl2(Omega), vex(t2r(Omega), check=check))) else: raise ValueError("expecting a 3x3 or 4x4 matrix")
def step(self): # inputs are set if self.bd.options.graphics: self.xdata.append(self.inputs[0]) self.ydata.append(self.inputs[1]) #plt.figure(self.fig.number) if self.path: self.line.set_data(self.xdata, self.ydata) T = base.transl2(self.inputs[0], self.inputs[1]) @ base.trot2( self.inputs[2]) new = base.h2e(T @ self.vertices_hom) self.vehicle.set_xy(new.T) if self.bd.options.animation: self.fig.canvas.flush_events() if self.scale == 'auto': self.ax.relim() self.ax.autoscale_view() super().step()
def step(self): # inputs are set if self.sim.graphics: self.xdata.append(self.inputs[0]) self.ydata.append(self.inputs[1]) plt.figure(self.fig.number) if self.path: self.line.set_data(self.xdata, self.ydata) T = sm.transl2(self.inputs[0], self.inputs[1]) @ sm.trot2( self.inputs[2]) new = sm.h2e(T @ self.vertices_hom) self.vehicle.set_xy(new.T) plt.draw() plt.show(block=False) if self.sim.animation: self.fig.canvas.start_event_loop(0.001) if self.scale == 'auto': self.ax.relim() self.ax.autoscale_view()
def Ty(cls, y): """ Create an SE(2) translation along the Y-axis :param y: translation distance along the Y-axis :type y: float :return: SE(2) matrix :rtype: SE2 instance `SE2.Ty(y) is an SE(2) translation of ``y`` along the y-axis Example: .. runblock:: pycon >>> SE2.Ty(2) >>> SE2.Ty([2,3]) :seealso: :func:`~spatialmath.base.transforms3d.transl` :SymPy: supported """ return cls([base.transl2(0, _y) for _y in base.getvector(y)], check=False)
def Tx(cls, x): """ Create an SE(2) translation along the X-axis :param x: translation distance along the X-axis :type x: float :return: SE(2) matrix :rtype: SE2 instance `SE2.Tx(x)` is an SE(2) translation of ``x`` along the x-axis Example: .. runblock:: pycon >>> SE2.Tx(2) >>> SE2.Tx([2,3]) :seealso: :func:`~spatialmath.base.transforms3d.transl` :SymPy: supported """ return cls([base.transl2(_x, 0) for _x in base.getvector(x)], check=False)
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')