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