示例#1
0
    def exp(self, theta=None, units='rad'):
        """
        Twist.exp Convert twist to homogeneous transformation

        TW.exp is the homogeneous transformation equivalent to the twist (SE2 or SE3).

        TW.exp(THETA) as above but with a rotation of THETA about the twist.

        Notes::
        - For the second form the twist must, if rotational, have a unit rotational component.

        See also Twist.T, trexp, trexp2.
        """

        if units != 'rad' and self.isprismatic:
            print('Twist.exp: using degree mode for a prismatic twist')

        if theta is None:
            theta = 1
        else:
            theta = argcheck.getunit(theta, units)

        if isinstance(theta, (int, np.int64, float, np.float64)):
            return SE2(tr.trexp2(self.S * theta))
        else:
            return SE2([tr.trexp2(self.S * t) for t in theta])
示例#2
0
    def draw(self):
        if not self.display:
            return

        if not self.drawn:
            self.init()
            return

        ## Update the robot links

        # compute all link frames
        T = self.robot.fkine_all(self.robot.q)

        # draw all the line segments for the noodle plot
        for i, segment in enumerate(self.segments):
            linkframes = []
            for link in segment:
                if link is None:
                    linkframes.append(self.robot.base)
                else:
                    linkframes.append(T[link.number])
            points = np.array([linkframe.t for linkframe in linkframes])

            self.links[i].set_xdata(points[:, 0])
            self.links[i].set_ydata(points[:, 1])

        ## Draw the end-effectors

        # Remove old ee coordinate frame
        if self.eeframes:
            for quiver in self.eeframes:
                quiver.remove()

            self.eeframes = []

        if self.eeframe:
            len = self.options['eelength']
            Tjx = SE2(len, 0)
            Tjy = SE2(0, len)

            # add new ee coordinate frame
            for link in self.robot.ee_links:
                Te = T[link.number]

                # ee axes arrows
                Tex = Te * Tjx
                Tey = Te * Tjy

                xaxis = self._plot_quiver(Te.t, Tex.t, self.options['eex'])
                yaxis = self._plot_quiver(Te.t, Tey.t, self.options['eey'])

                self.eeframes.extend([xaxis, yaxis])
    def draw(self):
        if not self.display:
            return

        if not self.drawn:
            self.init()
            return

        ## Update the robot links

        # compute all link frames
        T = self.robot.fkine_path(self.robot.q)

        # draw all the line segments for the noodle plot
        for i, segment in enumerate(self.segments):
            linkframes = []
            for link in segment:
                if link is None:
                    linkframes.append(self.robot.base)
                else:
                    linkframes.append(T[link.number + 1])
            points = np.array([linkframe.t for linkframe in linkframes])

            self.links[i].set_xdata(points[:, 0])
            self.links[i].set_ydata(points[:, 1])

        ## Draw the end-effectors

        # Remove old ee coordinate frame
        if self.eeframes:
            for quiver in self.eeframes:
                quiver.remove()

            self.eeframes = []

        Tjx = SE2(0.06, 0)
        Tjy = SE2(0, 0.06)
        red = '#F84752'  # '#EE9494'
        green = '#BADA55'  # '#93E7B0'

        # add new ee coordinate frame
        for link in self.robot.ee_links:
            Te = T[link.number + 1]

            # ee axes arrows
            Tex = Te * Tjx
            Tey = Te * Tjy

            xaxis = self._plot_quiver(Te.t, Tex.t, red, 2)
            yaxis = self._plot_quiver(Te.t, Tey.t, green, 2)

            self.eeframes.extend([xaxis, yaxis])
示例#4
0
def create_marker(scene, x, y, shape, colour=None):
    """
    Draw the shape at the given position

    :param scene: The scene in which to draw the object
    :type scene: class:`vpython.canvas`
    :param x: The x location to draw the object at
    :type x: `float`
    :param y: The y location to draw the object at
    :type y: `float`
    :param shape: The shape of the object to draw
    :type shape: `str`
    :param colour: The colour of the object
    :type colour: `list`
    :returns: A 2D object that has been drawn
    :rtype: class:`graphics.graphics_object2d.Object2D`
    """
    # Set default colour
    # Stops a warning about mutable parameter
    if colour is None:
        colour = [0, 0, 0]

    # Create an SE2 for the object
    obj_se2 = SE2(x=x, y=y, theta=0)

    # Create the object and return it
    return Marker2D(obj_se2, scene, shape, colour)
示例#5
0
    def SE2(tw):
        """
        %Twist.SE Convert twist to SE2 or SE3 object
        %
        TW.SE is an SE2 or SE3 object representing the homogeneous transformation equivalent to the twist.
                %
            See also Twist.T, SE2, SE3.
        """

        return SE2(tw.exp())
示例#6
0
    def A(self, q=0.0, **kwargs):
        """
        Link transform matrix
        :param q: Joint coordinate (radians or metres). Not required for links
            with no variable
        :type q: float
        :param fast: return NumPy array instead of ``SE3``
        :type param: bool
        :return T: link frame transformation matrix
        :rtype T: SE3 or ndarray(4,4)
        ``LINK.A(q)`` is an SE(3) matrix that describes the rigid-body
          transformation from the previous to the current link frame to
          the next, which depends on the joint coordinate ``q``.
        If ``fast`` is True return a NumPy array, either SE(2) or SE(3).
        A value of None means that it is the identity matrix.
        If ``fast`` is False return an ``SE2`` or ``SE3`` instance.
        """

        if self.isjoint:
            # a variable joint
            if q is None:
                raise ValueError("q is required for variable joints")

            # premultiply variable part by constant part if present
            Ts = self.Ts
            if Ts is None:
                T = self.v.T(q)
            else:
                T = Ts @ self.v.T(q)
        else:
            # a fixed joint
            T = self.Ts

        if T is None:
            return SE2()
        else:
            return SE2(T, check=False)
示例#7
0
    def test_fkine_all(self):
        a1 = 1
        a2 = 1
        e = ETS2.r() * ETS2.tx(a1) * ETS2.r() * ETS2.tx(a2)
        robot = ERobot2(e)

        T = robot.fkine_all([0, 0])
        self.assertIsInstance(T, SE2)
        self.assertEqual(len(T), 4)

        nt.assert_array_almost_equal(T[0].A, SE2().A)
        nt.assert_array_almost_equal(T[1].A, SE2().A)
        nt.assert_array_almost_equal(T[2].A, SE2(1, 0).A)
        nt.assert_array_almost_equal(T[3].A, SE2(2, 0).A)

        robot = ERobot2([
            ELink2(ETS2.r(), name='link1'),
            ELink2(ETS2.tx(1.2) * ETS2.ty(-0.5) * ETS2.r(),
                   name='link2',
                   parent='link1'),
            ELink2(ETS2.tx(1), name='ee_1', parent='link2'),
            ELink2(ETS2.tx(0.6) * ETS2.ty(0.5) * ETS2.r(),
                   name='link3',
                   parent='link1'),
            ELink2(ETS2.tx(1), name='ee_2', parent='link3')
        ])
        T = robot.fkine_all([0, 0, 0])
        self.assertIsInstance(T, SE2)
        self.assertEqual(len(T), 6)

        nt.assert_array_almost_equal(T[0].A, SE2().A)
        nt.assert_array_almost_equal(T[1].A, SE2().A)
        nt.assert_array_almost_equal(T[2].A, SE2(1.2, -0.5).A)
        nt.assert_array_almost_equal(T[3].A, SE2(2.2, -0.5).A)
        nt.assert_array_almost_equal(T[4].A, SE2(0.6, 0.5).A)
        nt.assert_array_almost_equal(T[5].A, SE2(1.6, 0.5).A)
    def scanmap(self,
                centre=(75, 50),
                ngrid=3000,
                cellsize=0.1,
                maxrange=None):

        self._centre = centre
        self._cellsize = cellsize
        self._ngrid = ngrid

        # h = waitbar(0, 'rendering a map')

        world = np.zeros((ngrid, ngrid), np.int32)
        for i in range(0, self.graph.n):

            # if i % 20 == 0:
            #     waitbar(i/self.graph.n, h)

            xy = self.scanxy(i)
            r, theta = self.scan(i)
            if maxrange is not None:
                xy = np.delete(xy, r > maxrange, axis=0)
            xyt = self.vindex[i].coord

            xy = SE2(xyt) * xy.T

            # start of each ray
            p1 = self.w2g(xyt[0:2])

            for end in xy.T:

                # end of each ray
                p2 = self.w2g(end[0:2])

                # all cells along the ray
                x, y = bresenham(p1, p2)

                try:
                    # decrement cells along the ray, these are free space
                    world[y[:-1], x[:-1]] = world[y[:-1], x[:-1]] - 1
                    # increment target cell
                    world[y[-1], x[-1]] = world[y[-1], x[-1]] + 1
                except IndexError:
                    # silently ignore rays to points outside the grid map
                    pass

        return world
示例#9
0
    def ty(cls, eta=None):
        """
        An elementary transform (ET). A pure translation of eta along the
        x-axis

        L = Tty(eta) will instantiate an ET object which represents a pure
        translation along the y-axis by amount eta.

        L = Tty() as above except this ET representation a variable
        translation, i.e. a joint

        :param eta: The amount of translation along the x-axis
        :type eta: float (metres)
        :return: An ET object
        :rtype: ET

        """
        return cls(lambda y: SE2(0, y), axis='ty', eta=eta)
示例#10
0
    def r(cls, eta=None):
        """
        An elementary transform (ET). A pure rotation of eta about the x-axis.

        L = TRx(eta) will instantiate an ET object which represents a pure
        rotation about the x-axis by amount eta.

        L = TRx() as above except this ET representation a variable
        rotation, i.e. a joint

        :param eta: The amount of rotation about the x-axis
        :type eta: float (radians)
        :param joint: The joint number within the robot
        :type joint: int
        :return: An ET object
        :rtype: ET

        """
        return cls(lambda theta: SE2(theta), axis='R', eta=eta)
示例#11
0
    def eval(self, q):
        """
        Evaluate an ETS with joint coordinate substitution

        :param q: joint coordinates
        :type q: array-like
        :return: product of transformations
        :rtype: SE2

        Effectively the forward-kinematics of the ET sequence.  Compounds the
        transforms left to right, and substitutes in joint coordinates as 
        needed from consecutive elements of ``q``.
        """
        T = SE2()

        q = getvector(q, out='sequence')
        for e in self:
            if e.jtype == self.VARIABLE:
                T *= e.T(q.pop(0))
            else:
                T *= e.T()
        return T
    def _update(self, x):

        xy = SE2(x) * self._coords
        self._object.set_xy(xy.T)
def test_2d_create_object():
    g_canvas2 = gph.GraphicsCanvas2D()

    obj = gph.Object2D(SE2(), g_canvas2.scene, 'c')
示例#14
0
    def eval(self, q=None, unit='rad'):
        """
        Evaluate an ETS with joint coordinate substitution

        :param q: joint coordinates
        :type q: array-like
        :param unit: angular unit, "rad" [default] or "deg"
        :type unit: str
        :return: The SE(3) or SE(2) matrix value of the ET sequence
        :rtype:  ndarray(4,4) or ndarray(3,3)

        Effectively the forward-kinematics of the ET sequence.  Compounds the
        transforms left to right, and substitutes in joint coordinates as
        needed from consecutive elements of ``q``.

        .. note:: if ETs have an explicit joint index, this is used to index
            into the vector ``q``.

        .. warning:: do not mix ETs with and without explicit joint index.

        Example:

        .. runblock:: pycon

            >>> from roboticstoolbox import ETS
            >>> e = ETS.rz() * ETS.tx(1) * ETS.rz() * ETS.tx(1)
            >>> print(e)
            >>> len(e)
            >>> e[1:3]
            >>> e.eval([0, 0])
            >>> e.eval([90, -90], 'deg')
            >>> e = ETS.tx(j=1) * ETS.ty(j=0)
            >>> e.eval([3, 4])
        """
        if q is not None:
            q = getvector(q, out='list')
        first = True
        for et in self:
            if et.isjoint:
                if et.jindex is None:
                    qj = q.pop(0)
                else:
                    qj = q[et.jindex]
                if et.isrevolute and unit == 'deg':
                    qj *= np.pi / 180.0
                if et.isflip:
                    qj = -qj
                Tk = et.T(qj)
            else:
                # for constants
                Tk = et.T()
            if first:
                T = Tk
                first = False
            else:
                T = T @ Tk

        if isinstance(self, ETS):
            T = SE3(T, check=False)
        elif isinstance(self, ETS2):
            T = SE2(T, check=False)
        T.simplify()
        return T
    #         pass
    #     else:
    #         raise ValueError('expecting SO3 or rotation matrix')
    #     if t is None:
    #         return cls(base.r2t(R))
    #     else:
    #         return cls(base.rt2tr(R, t))


if __name__ == '__main__':  # pragma: no cover

    import pathlib

    a = SE3(1, 2, 3)
    b = SO3(a)
    print(b)

    a = SO3.RPY([.1, .2, .3])
    b = SE3(a)
    print(b)

    from spatialmath import SE2
    a = SE2(1, 2, .4)
    b = SE3(a)
    print(b)

    exec(
        open(
            pathlib.Path(__file__).parent.parent.absolute() / "tests" /
            "test_pose3d.py").read())  # pylint: disable=exec-used
示例#16
0
    def test_constructor(self):

        # null constructor
        R = SE3()
        nt.assert_equal(len(R), 1)
        array_compare(R, np.eye(4))
        self.assertIsInstance(R, SE3)

        # construct from matrix
        R = SE3(trotx(0.2))
        nt.assert_equal(len(R), 1)
        array_compare(R, trotx(0.2))
        self.assertIsInstance(R, SE3)

        # construct from canonic rotation
        R = SE3.Rx(0.2)
        nt.assert_equal(len(R), 1)
        array_compare(R, trotx(0.2))
        self.assertIsInstance(R, SE3)

        R = SE3.Ry(0.2)
        nt.assert_equal(len(R), 1)
        array_compare(R, troty(0.2))
        self.assertIsInstance(R, SE3)

        R = SE3.Rz(0.2)
        nt.assert_equal(len(R), 1)
        array_compare(R, trotz(0.2))
        self.assertIsInstance(R, SE3)

        # construct from canonic translation
        R = SE3.Tx(0.2)
        nt.assert_equal(len(R), 1)
        array_compare(R, transl(0.2, 0, 0))
        self.assertIsInstance(R, SE3)

        R = SE3.Ty(0.2)
        nt.assert_equal(len(R), 1)
        array_compare(R, transl(0, 0.2, 0))
        self.assertIsInstance(R, SE3)

        R = SE3.Tz(0.2)
        nt.assert_equal(len(R), 1)
        array_compare(R, transl(0, 0, 0.2))
        self.assertIsInstance(R, SE3)

        # triple angle
        R = SE3.Eul([0.1, 0.2, 0.3])
        nt.assert_equal(len(R), 1)
        array_compare(R, eul2tr([0.1, 0.2, 0.3]))
        self.assertIsInstance(R, SE3)

        R = SE3.Eul(np.r_[0.1, 0.2, 0.3])
        nt.assert_equal(len(R), 1)
        array_compare(R, eul2tr([0.1, 0.2, 0.3]))
        self.assertIsInstance(R, SE3)

        R = SE3.Eul([10, 20, 30], unit='deg')
        nt.assert_equal(len(R), 1)
        array_compare(R, eul2tr([10, 20, 30], unit='deg'))
        self.assertIsInstance(R, SE3)

        R = SE3.RPY([0.1, 0.2, 0.3])
        nt.assert_equal(len(R), 1)
        array_compare(R, rpy2tr([0.1, 0.2, 0.3]))
        self.assertIsInstance(R, SE3)

        R = SE3.RPY(np.r_[0.1, 0.2, 0.3])
        nt.assert_equal(len(R), 1)
        array_compare(R, rpy2tr([0.1, 0.2, 0.3]))
        self.assertIsInstance(R, SE3)

        R = SE3.RPY([10, 20, 30], unit='deg')
        nt.assert_equal(len(R), 1)
        array_compare(R, rpy2tr([10, 20, 30], unit='deg'))
        self.assertIsInstance(R, SE3)

        R = SE3.RPY([0.1, 0.2, 0.3], order='xyz')
        nt.assert_equal(len(R), 1)
        array_compare(R, rpy2tr([0.1, 0.2, 0.3], order='xyz'))
        self.assertIsInstance(R, SE3)

        # angvec
        R = SE3.AngVec(0.2, [1, 0, 0])
        nt.assert_equal(len(R), 1)
        array_compare(R, trotx(0.2))
        self.assertIsInstance(R, SE3)

        R = SE3.AngVec(0.3, [0, 1, 0])
        nt.assert_equal(len(R), 1)
        array_compare(R, troty(0.3))
        self.assertIsInstance(R, SE3)

        # OA
        R = SE3.OA([0, 1, 0], [0, 0, 1])
        nt.assert_equal(len(R), 1)
        array_compare(R, np.eye(4))
        self.assertIsInstance(R, SE3)

        # random
        R = SE3.Rand()
        nt.assert_equal(len(R), 1)
        self.assertIsInstance(R, SE3)

        # random
        T = SE3.Rand()
        R = T.R
        t = T.t
        T = SE3.Rt(R, t)
        self.assertIsInstance(T, SE3)
        self.assertEqual(T.A.shape, (4, 4))

        nt.assert_equal(T.R, R)
        nt.assert_equal(T.t, t)

        # copy constructor
        R = SE3.Rx(pi / 2)
        R2 = SE3(R)
        R = SE3.Ry(pi / 2)
        array_compare(R2, trotx(pi / 2))

        # SO3
        T = SE3(SO3())
        nt.assert_equal(len(T), 1)
        self.assertIsInstance(T, SE3)
        nt.assert_equal(T.A, np.eye(4))

        # SE2
        T = SE3(SE2(1, 2, 0.4))
        nt.assert_equal(len(T), 1)
        self.assertIsInstance(T, SE3)
        self.assertEqual(T.A.shape, (4, 4))
        nt.assert_equal(T.t, [1, 2, 0])