roboticstoolbox-python 1.3.0__cp313-cp313-win_amd64.whl

roboticstoolbox/models/DH/P8.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python
+"""
+@author: Peter Corke
+@author: Jesse Haviland
+"""
+
+# 2/8/95  changed D3 to 150.05mm which is closer to data from Lee, AKB86 and
+# Tarn fixed errors in COG for links 2 and 3
+# 29/1/91 to agree with data from Armstrong etal.  Due to their use
+#  of modified D&H params, some of the offsets Ai, Di are
+#  offset, and for links 3-5 swap Y and Z axes.
+# 14/2/91 to use Paul's value of link twist (alpha) to be consistant
+#  with ARCL.  This is the -ve of Lee's values, which means the
+#  zero angle position is a righty for Paul, and lefty for Lee.
+
+# all parameters are in SI units: m, radians, kg, kg.m2, N.m, N.m.s etc.
+
+# from math import pi
+import numpy as np
+from roboticstoolbox import DHRobot, PrismaticDH, models
+from spatialmath import SE3
+from spatialmath import base
+from math import pi
+
+
+class P8(DHRobot):
+    """
+    Class that models a Puma robot on an XY base
+
+    ``P8()`` is an object which models an 8-axis robot comprising a Puma 560
+    robot on an XY base.  Joints 0 and 1 are the base, joints 2-7 are the robot
+    arm.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.DH.Puma560()
+        >>> print(robot)
+
+    Defined joint configurations are:
+
+    - qz, zero joint angle configuration, 'L' shaped configuration
+
+
+    .. note::
+        - SI units are used.
+
+    .. codeauthor:: Peter Corke
+
+    :seealso: :func:`models.DH.Puma560`
+    """  # noqa
+
+    def __init__(self):
+
+        # create the base
+        links = [
+            PrismaticDH(alpha=-pi / 2, qlim=[-1, 1]),
+            PrismaticDH(theta=-pi / 2, alpha=pi / 2, qlim=[-1, 1]),
+        ]
+
+        # stick the Puma on top
+        puma = models.DH.Puma560()
+        links.extend(puma.links)
+
+        super().__init__(
+            links, name="P8", keywords=("mobile", "redundant"), base=SE3.Ry(pi / 2)
+        )
+
+        self.qr = np.zeros(8)
+        self.qz = np.zeros(8)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+
+if __name__ == "__main__":  # pragma nocover
+    robot = P8()
+    print(robot)
+
+    # robot.plot([.5, -0.6, 0, 0, 0, 0, 0, 0], block=True)

roboticstoolbox/models/DH/Panda.py

@@ -0,0 +1,178 @@
+#!/usr/bin/env python
+
+import numpy as np
+from spatialmath.base import trotz, transl
+from roboticstoolbox import DHRobot, RevoluteMDH
+
+
+class Panda(DHRobot):
+    """
+    A class representing the Panda robot arm.
+
+    ``Panda()`` is a class which models a Franka-Emika Panda robot and
+    describes its kinematic characteristics using modified DH
+    conventions.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.DH.Panda()
+        >>> print(robot)
+
+    .. note::
+        - SI units of metres are used.
+        - The model includes a tool offset.
+
+    :references:
+        - https://frankaemika.github.io/docs/control_parameters.html
+
+    .. codeauthor:: Samuel Drew
+    .. codeauthor:: Peter Corke
+    """
+
+    def __init__(self):
+
+        # deg = np.pi/180
+        mm = 1e-3
+        tool_offset = (103) * mm
+
+        flange = (107) * mm
+        # d7 = (58.4)*mm
+
+        # This Panda model is defined using modified
+        # Denavit-Hartenberg parameters
+        L = [
+            RevoluteMDH(
+                a=0.0,
+                d=0.333,
+                alpha=0.0,
+                qlim=np.array([-2.8973, 2.8973]),
+                m=4.970684,
+                I=[
+                    7.03370e-01,
+                    7.06610e-01,
+                    9.11700e-03,
+                    -1.39000e-04,
+                    1.91690e-02,
+                    6.77200e-03,
+                ],
+                G=1,
+            ),
+            RevoluteMDH(
+                a=0.0,
+                d=0.0,
+                alpha=-np.pi / 2,
+                qlim=np.array([-1.7628, 1.7628]),
+                m=0.646926,
+                I=[
+                    7.96200e-03,
+                    2.81100e-02,
+                    2.59950e-02,
+                    -3.92500e-03,
+                    7.04000e-04,
+                    1.02540e-02,
+                ],
+                G=1,
+            ),
+            RevoluteMDH(
+                a=0.0,
+                d=0.316,
+                alpha=np.pi / 2,
+                qlim=np.array([-2.8973, 2.8973]),
+                m=3.228604,
+                I=[
+                    3.72420e-02,
+                    3.61550e-02,
+                    1.08300e-02,
+                    -4.76100e-03,
+                    -1.28050e-02,
+                    -1.13960e-02,
+                ],
+                G=1,
+            ),
+            RevoluteMDH(
+                a=0.0825,
+                d=0.0,
+                alpha=np.pi / 2,
+                qlim=np.array([-3.0718, -0.0698]),
+                m=3.587895,
+                I=[
+                    2.58530e-02,
+                    1.95520e-02,
+                    2.83230e-02,
+                    7.79600e-03,
+                    8.64100e-03,
+                    -1.33200e-03,
+                ],
+                G=1,
+            ),
+            RevoluteMDH(
+                a=-0.0825,
+                d=0.384,
+                alpha=-np.pi / 2,
+                qlim=np.array([-2.8973, 2.8973]),
+                m=1.225946,
+                I=[
+                    3.55490e-02,
+                    2.94740e-02,
+                    8.62700e-03,
+                    -2.11700e-03,
+                    2.29000e-04,
+                    -4.03700e-03,
+                ],
+                G=1,
+            ),
+            RevoluteMDH(
+                a=0.0,
+                d=0.0,
+                alpha=np.pi / 2,
+                qlim=np.array([-0.0175, 3.7525]),
+                m=1.666555,
+                I=[
+                    1.96400e-03,
+                    4.35400e-03,
+                    5.43300e-03,
+                    1.09000e-04,
+                    3.41000e-04,
+                    -1.15800e-03,
+                ],
+                G=1,
+            ),
+            RevoluteMDH(
+                a=0.088,
+                d=flange,
+                alpha=np.pi / 2,
+                qlim=np.array([-2.8973, 2.8973]),
+                m=7.35522e-01,
+                I=[
+                    1.25160e-02,
+                    1.00270e-02,
+                    4.81500e-03,
+                    -4.28000e-04,
+                    -7.41000e-04,
+                    -1.19600e-03,
+                ],
+                G=1,
+            ),
+        ]
+
+        tool = transl(0, 0, tool_offset) @ trotz(-np.pi / 4)
+
+        super().__init__(
+            L,
+            name="Panda",
+            manufacturer="Franka Emika",
+            meshdir="meshes/FRANKA-EMIKA/Panda",
+            tool=tool,
+        )
+
+        self.qr = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4])
+        self.qz = np.zeros(7)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+
+if __name__ == "__main__":  # pragma nocover
+    panda = Panda()
+    print(panda)

roboticstoolbox/models/DH/Planar2.py

@@ -0,0 +1,69 @@
+"""
+@author: Luis Fernando Lara Tobar
+@author: Peter Corke
+@author: Samuel Drew
+"""
+
+from roboticstoolbox import DHRobot, RevoluteDH
+import numpy as np
+
+
+class Planar2(DHRobot):
+    """
+    Class that models a planar 2-link robot
+
+    ``Planar2()`` is a class which models a 2-link planar robot and
+    describes its kinematic characteristics using standard DH
+    conventions.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.DH.Planar2()
+        >>> print(robot)
+
+    Defined joint configurations are:
+
+        - qz, zero angles, all folded up
+        - q1, links are horizontal and vertical respectively
+        - q2, links are vertical and horizontal respectively
+
+    .. note::
+
+      - Robot has only 2 DoF.
+
+    .. codeauthor:: Peter Corke
+    """
+
+    def __init__(self, symbolic=False):
+
+        if symbolic:
+            import spatialmath.base.symbolic as sym
+
+            zero = sym.zero()
+            pi = sym.pi()
+            a1, a2 = sym.symbol("a1 a2")  # type: ignore
+        else:
+            from math import pi
+
+            zero = 0.0
+            a1 = 1
+            a2 = 1
+
+        L = [RevoluteDH(a=a1, alpha=zero), RevoluteDH(a=a2, alpha=zero)]
+
+        super().__init__(L, name="Planar 2 link", keywords=("planar",))
+
+        self.qr = np.array([0, pi / 2])
+        self.qz = np.zeros(2)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+        self.addconfiguration_attr("q1", np.array([0, pi / 2]))
+        self.addconfiguration_attr("q2", np.array([pi / 2, -pi / 2]))
+
+
+if __name__ == "__main__":  # pragma nocover
+    robot = Planar2()
+    print(robot)

roboticstoolbox/models/DH/Planar3.py

@@ -0,0 +1,51 @@
+"""
+@author: Luis Fernando Lara Tobar
+@author: Peter Corke
+@author: Samuel Drew
+"""
+
+from roboticstoolbox import DHRobot, RevoluteDH
+import numpy as np
+
+
+class Planar3(DHRobot):
+    """
+    Class that models a planar 3-link robot
+
+    ``Planar2()`` is a class which models a 3-link planar robot and
+    describes its kinematic characteristics using standard DH
+    conventions.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.DH.Planar3()
+        >>> print(robot)
+
+    Defined joint configurations are:
+
+        - qz, zero angles, all folded up
+
+    .. note::
+
+      - Robot has only 3 DoF.
+
+    .. codeauthor:: Peter Corke
+    """
+
+    def __init__(self):
+
+        L = [RevoluteDH(a=1), RevoluteDH(a=1), RevoluteDH(a=1)]
+
+        super().__init__(L, name="Planar 3 link", keywords=("planar",))
+
+        self.qr = np.zeros(3)
+        self.qz = np.zeros(3)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+
+if __name__ == "__main__":  # pragma nocover
+    robot = Planar3()
+    print(robot)

roboticstoolbox/models/DH/Puma560.py

@@ -0,0 +1,326 @@
+#!/usr/bin/env python
+"""
+@author: Peter Corke
+@author: Jesse Haviland
+"""
+
+# 2/8/95  changed D3 to 150.05mm which is closer to data from Lee, AKB86 and
+# Tarn fixed errors in COG for links 2 and 3
+# 29/1/91 to agree with data from Armstrong etal.  Due to their use
+#  of modified D&H params, some of the offsets Ai, Di are
+#  offset, and for links 3-5 swap Y and Z axes.
+# 14/2/91 to use Paul's value of link twist (alpha) to be consistant
+#  with ARCL.  This is the -ve of Lee's values, which means the
+#  zero angle position is a righty for Paul, and lefty for Lee.
+
+# all parameters are in SI units: m, radians, kg, kg.m2, N.m, N.m.s etc.
+
+# from math import pi
+import numpy as np
+from roboticstoolbox import DHRobot, RevoluteDH
+from spatialmath import SE3
+from spatialmath import base
+
+
+class Puma560(DHRobot):
+    """
+    Class that models a Puma 560 manipulator
+
+    :param symbolic: use symbolic constants
+    :type symbolic: bool
+
+    ``Puma560()`` is an object which models a Unimation Puma560 robot and
+    describes its kinematic and dynamic characteristics using standard DH
+    conventions.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.DH.Puma560()
+        >>> print(robot)
+
+    Defined joint configurations are:
+
+    - qz, zero joint angle configuration, 'L' shaped configuration
+    - qr, vertical 'READY' configuration
+    - qs, arm is stretched out in the x-direction
+    - qn, arm is at a nominal non-singular configuration
+
+    .. note::
+        - SI units are used.
+        - The model includes armature inertia and gear ratios.
+        - The value of m1 is given as 0 here.  Armstrong found no value for it
+          and it does not appear in the equation for tau1 after the
+          substituion is made to inertia about link frame rather than COG
+          frame.
+        - Gravity load torque is the motor torque necessary to keep the joint
+          static, and is thus -ve of the gravity caused torque.
+
+    .. warning:: Compared to the MATLAB version of the Toolbox this model
+        includes the pedestal, making the z-coordinates 26 inches larger.
+
+    :references:
+        - "A search for consensus among model parameters reported for the PUMA
+          560 robot", P. Corke and B. Armstrong-Helouvry,
+          Proc. IEEE Int. Conf. Robotics and Automation, (San Diego),
+          pp. 1608-1613, May 1994. (for kinematic and dynamic parameters)
+        - "A combined optimization method for solving the inverse kinematics
+          problem", Wang & Chen, IEEE Trans. RA 7(4) 1991 pp 489-.
+          (for joint angle limits)
+        - https://github.com/4rtur1t0/ARTE/blob/master/robots/UNIMATE/puma560/parameters.m
+
+    .. codeauthor:: Peter Corke
+    """  # noqa
+
+    def __init__(self, symbolic=False):
+
+        if symbolic:
+            import spatialmath.base.symbolic as sym
+
+            zero = sym.zero()
+            pi = sym.pi()
+        else:
+            from math import pi
+
+            zero = 0.0
+
+        deg = pi / 180
+        inch = 0.0254
+
+        base = 26.45 * inch  # from mounting surface to shoulder axis
+
+        L = [
+            RevoluteDH(
+                d=base,  # link length (Dennavit-Hartenberg notation)
+                a=0,  # link offset (Dennavit-Hartenberg notation)
+                alpha=pi / 2,  # link twist (Dennavit-Hartenberg notation)
+                I=[0, 0.35, 0, 0, 0, 0],
+                # inertia tensor of link with respect to
+                # center of mass I = [L_xx, L_yy, L_zz,
+                # L_xy, L_yz, L_xz]
+                r=[0, 0, 0],
+                # distance of ith origin to center of mass [x,y,z]
+                # in link reference frame
+                m=0,  # mass of link
+                Jm=200e-6,  # actuator inertia
+                G=-62.6111,  # gear ratio
+                B=1.48e-3,  # actuator viscous friction coefficient (measured
+                # at the motor)
+                Tc=[0.395, -0.435],
+                # actuator Coulomb friction coefficient for
+                # direction [-,+] (measured at the motor)
+                qlim=[-160 * deg, 160 * deg],  # minimum and maximum joint angle
+            ),
+            RevoluteDH(
+                d=0,
+                a=0.4318,
+                alpha=zero,
+                I=[0.13, 0.524, 0.539, 0, 0, 0],
+                r=[-0.3638, 0.006, 0.2275],
+                m=17.4,
+                Jm=200e-6,
+                G=107.815,
+                B=0.817e-3,
+                Tc=[0.126, -0.071],
+                qlim=[-110 * deg, 110 * deg],  # qlim=[-45*deg, 225*deg]
+            ),
+            RevoluteDH(
+                d=0.15005,
+                a=0.0203,
+                alpha=-pi / 2,
+                I=[0.066, 0.086, 0.0125, 0, 0, 0],
+                r=[-0.0203, -0.0141, 0.070],
+                m=4.8,
+                Jm=200e-6,
+                G=-53.7063,
+                B=1.38e-3,
+                Tc=[0.132, -0.105],
+                qlim=[-135 * deg, 135 * deg],  # qlim=[-225*deg, 45*deg]
+            ),
+            RevoluteDH(
+                d=0.4318,
+                a=0,
+                alpha=pi / 2,
+                I=[1.8e-3, 1.3e-3, 1.8e-3, 0, 0, 0],
+                r=[0, 0.019, 0],
+                m=0.82,
+                Jm=33e-6,
+                G=76.0364,
+                B=71.2e-6,
+                Tc=[11.2e-3, -16.9e-3],
+                qlim=[-266 * deg, 266 * deg],  # qlim=[-110*deg, 170*deg]
+            ),
+            RevoluteDH(
+                d=0,
+                a=0,
+                alpha=-pi / 2,
+                I=[0.3e-3, 0.4e-3, 0.3e-3, 0, 0, 0],
+                r=[0, 0, 0],
+                m=0.34,
+                Jm=33e-6,
+                G=71.923,
+                B=82.6e-6,
+                Tc=[9.26e-3, -14.5e-3],
+                qlim=[-100 * deg, 100 * deg],
+            ),
+            RevoluteDH(
+                d=0,
+                a=0,
+                alpha=zero,
+                I=[0.15e-3, 0.15e-3, 0.04e-3, 0, 0, 0],
+                r=[0, 0, 0.032],
+                m=0.09,
+                Jm=33e-6,
+                G=76.686,
+                B=36.7e-6,
+                Tc=[3.96e-3, -10.5e-3],
+                qlim=[-266 * deg, 266 * deg],
+            ),
+        ]
+
+        super().__init__(
+            L,
+            name="Puma 560",
+            manufacturer="Unimation",
+            keywords=("dynamics", "symbolic", "mesh"),
+            symbolic=symbolic,
+            meshdir="meshes/UNIMATE/puma560",
+        )
+
+        self.qr = np.array([0, pi / 2, -pi / 2, 0, 0, 0])
+        self.qz = np.zeros(6)
+
+        # nominal table top picking pose
+        self.qn = np.array([0, pi / 4, pi, 0, pi / 4, 0])
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+        self.addconfiguration("qn", self.qn)
+
+        # straight and horizontal
+        self.addconfiguration_attr("qs", np.array([0, 0, -pi / 2, 0, 0, 0]))
+
+    def ikine_a(self, T, config="lun"):
+        """
+        Analytic inverse kinematic solution
+
+        :param T: end-effector pose
+        :type T: SE3
+        :param config: arm configuration, defaults to "lun"
+        :type config: str, optional
+        :return: joint angle vector in radians
+        :rtype: ndarray(6)
+
+        ``robot.ikine_a(T, config)`` is the joint angle vector which achieves the
+        end-effector pose ``T```.  The configuration string selects the specific
+        solution and is a sting comprising the following letters:
+
+        ======   ==============================================
+        Letter   Meaning
+        ======   ==============================================
+        l        Choose the left-handed configuration
+        r        Choose the right-handed configuration
+        u        Choose the elbow up configuration
+        d        Choose the elbow down configuration
+        n        Choose the wrist not-flipped configuration
+        f        Choose the wrist flipped configuration
+        ======   ==============================================
+
+
+        :reference:
+            - Inverse kinematics for a PUMA 560,
+              Paul and Zhang,
+              The International Journal of Robotics Research,
+              Vol. 5, No. 2, Summer 1986, p. 32-44
+
+        :author: based on MATLAB code by Robert Biro with Gary Von McMurray,
+            GTRI/ATRP/IIMB, Georgia Institute of Technology, 2/13/95
+
+        """
+
+        def ik3(robot, T, config="lun"):
+
+            config = self.config_validate(config, ("lr", "ud", "nf"))
+
+            # solve for the first three joints
+
+            a2 = robot.links[1].a
+            a3 = robot.links[2].a
+            d1 = robot.links[0].d
+            d3 = robot.links[2].d
+            d4 = robot.links[3].d
+
+            # The following parameters are extracted from the Homogeneous
+            # Transformation as defined in equation 1, p. 34
+
+            Px, Py, Pz = T.t
+            Pz -= d1  # offset the pedestal height
+            theta = np.zeros((3,))
+
+            # Solve for theta[0]
+            # r is defined in equation 38, p. 39.
+            # theta[0] uses equations 40 and 41, p.39,
+            # based on the configuration parameter n1
+
+            r = np.sqrt(Px**2 + Py**2)
+            if "r" in config:
+                theta[0] = np.arctan2(Py, Px) + np.arcsin(d3 / r)
+            elif "l" in config:
+                theta[0] = np.arctan2(Py, Px) + np.pi - np.arcsin(d3 / r)
+            else:
+                raise ValueError("bad configuration string")
+
+            # Solve for theta[1]
+            # V114 is defined in equation 43, p.39.
+            # r is defined in equation 47, p.39.
+            # Psi is defined in equation 49, p.40.
+            # theta[1] uses equations 50 and 51, p.40, based on the
+            # configuration parameter n2
+            if "u" in config:
+                n2 = 1
+            elif "d" in config:
+                n2 = -1
+            else:
+                raise ValueError("bad configuration string")
+
+            if "l" in config:
+                n2 = -n2
+
+            V114 = Px * np.cos(theta[0]) + Py * np.sin(theta[0])
+
+            r = np.sqrt(V114**2 + Pz**2)
+
+            with np.errstate(invalid="raise"):
+                try:
+                    Psi = np.arccos(
+                        (a2**2 - d4**2 - a3**2 + V114**2 + Pz**2) / (2.0 * a2 * r)
+                    )
+                except FloatingPointError:
+                    return "Out of reach"
+
+            theta[1] = np.arctan2(Pz, V114) + n2 * Psi
+
+            # Solve for theta[2]
+            # theta[2] uses equation 57, p. 40.
+            num = np.cos(theta[1]) * V114 + np.sin(theta[1]) * Pz - a2
+            den = np.cos(theta[1]) * Pz - np.sin(theta[1]) * V114
+            theta[2] = np.arctan2(a3, d4) - np.arctan2(num, den)
+
+            theta = base.angdiff(theta)
+
+            return theta
+
+        return self.ikine_6s(T, config, ik3)
+
+
+if __name__ == "__main__":  # pragma nocover
+    puma = Puma560(symbolic=False)
+    print(puma)
+    print(puma.dynamics())
+    # T = puma.fkine(puma.qn)
+    # print(puma.ikine_a(T, 'lu').q)
+    # print(puma.ikine_a(T, 'ru').q)
+    # print(puma.ikine_a(T, 'ld').q)
+    # print(puma.ikine_a(T, 'rd').q)
+
+    # puma.plot(puma.qz)