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')
