def r(cls, eta=None, unit='rad', **kwargs):
"""
Pure rotation
:param η: rotation angle
:type η: float
:param unit: angular unit, "rad" [default] or "deg"
:type unit: str
: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.r(η)`` is an elementary rotation by a constant angle η
- ``ETS.r()`` is an elementary rotation by a variable angle, i.e. a
revolute robot joint. ``j`` or ``flip`` can be set in
this case.
.. note:: In the 2D case this is rotation around the normal to the
xy-plane.
:seealso: :func:`ETS`, :func:`isrevolute`
"""
return cls(
lambda theta: trot2(theta), axis='R', eta=eta, unit=unit, **kwargs)
def Rand(cls, *, xrange=(-1, 1), yrange=(-1, 1), arange=(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 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([tr.trot2(t, t=[x, y]) for (t, x, y) in zip(x, y, argcheck.getunit(theta, unit))])
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 = [tr.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 = [tr.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 = [tr.trot2(_t, t=[_x, _y], unit=unit) for (_x, _y, _t) in x]
else:
super().arghandler(x)
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 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 __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
elif argcheck.isscalar(x):
self.data = [tr.trot2(x, unit=unit)]
elif len(x) == 2:
# SE2([x,y])
self.data = [tr.transl2(x)]
elif len(x) == 3:
# SE2([x,y,theta])
self.data = [tr.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 = [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)]
else:
raise ValueError('bad arguments to constructor')
