roboticstoolbox-python 1.4.0__py3-none-any.whl

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)

roboticstoolbox/models/DH/README.md

@@ -0,0 +1,216 @@
+# Denavit-Hartenberg models
+
+## Shipped robot models
+
+| Model name | Description |
+| ---        | ---         |
+| Ball       | an n-link robot that folds into a ball shape |
+| Cobra600   | 4-axis Adept (now OMRON) SCARA robot |
+| IRB140  | 6-axis ABB robot |
+| KR5  | 6-axis Kuka robot |
+| Panda | 7-axis Franka-Emika robot |
+| Puma | 6-axis Unimation robot (with dynamics data) |
+| Stanford | 6-axis Stanford resarch robot, 1 prismatic joint (with dynamics data) |
+| Threelink | ?? |
+
+## Creating a new robot model using Denavit-Hartenberg parameters
+
+To begin, you must know:
+
+1. The DH parameters for your robot.
+2. Whether the model uses standard or modified Denavit-Hartenberg parameters, DH or MDH respectively.
+3. The joint structure, does it have revolute, prismatic joints or both.
+
+
+### Write the definition
+
+Create a file called `MYROBOT.py` where `MYROBOT` is a descriptive name of your
+robot that is a valid filename and Python class name.
+
+Cut and paste the code blocks below into your empty file, modifying as you go.  We start with the definitions:
+
+
+```python
+from math import pi
+import numpy as np
+from roboticstoolbox import DHRobot, RevoluteDH, PrismaticDH, RevoluteMDH, PrismaticMDH
+```
+
+The last line is important, it defines all the classes you could possibly
+need.  You won't use all of them, and to be tidy you could delete those you don't use.  This is their purpose:
+
+* `RevoluteDH` for a revolute joint using standard DH parameters
+* `PrismaticDH` for a prismatic joint using standard DH parameters
+* `RevoluteMDH` for a revolute joint using modified DH parameters
+* `PrismaticMDH` for a prismatic joint using modified DH parameters
+
+Next, add as much description as you can, check out the other model files in this
+folder for inspiration.
+
+```python
+class MYROBOT(DHRobot):
+    """
+    Create model of MYROBOT manipulator
+
+       .
+       .
+       .
+
+    :notes:
+       .
+       .
+       .
+
+    :references:
+       .
+       .
+       .     
+
+    """
+```
+
+Always a good idea to have notes about units used, DH convention, and
+to include any references to the source of your model.  
+
+Now for the main event:.  Inside the `__init__` method, define a set of link variables by creating instances of the appropriate link class.
+
+```python
+def __init__(self):
+
+        deg = pi/180
+
+        L0 = RevoluteDH(
+            d=0,          # 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
+
+        L1 = RevoluteDH(
+            d=0, a=0.4318, alpha=0,
+            qlim=[-45*deg, 225*deg])
+
+            .
+            .
+            .   
+
+```
+
+Provide as many parameters as you can.  The definition of `L0` above includes
+kinematic and dynamic parameters, whereas `L1` has only kinematic parameters.
+The minimum requirement is for the kinematic parameters, and you don't even need
+to know the joint limits, they are only required by a small number of Toolbox
+functions.
+
+For a robot with N joints you must define N joint instances.
+
+Next we call the superclass constructor to do the heavy lifting.
+
+```python
+        super().__init__(
+            [L0, L1, L2, L3, L4, L5],
+            name="MYROBOT",
+            manufacturer="COMPANY THAT BUILDS MYROBOTs")
+```
+
+We pass in an ordered list of all the joint objects we created earlier, and add 
+some metadata.  The name gets used in plots.
+
+We might like to define some useful joint configurations, maybe the home position, or where it goes to pick up a widget.  You can 
+define as many of these as you like, but the pattern looks like this:
+
+```python
+        # zero angles, L shaped pose
+        self._MYCONFIG = np.array([1, 2, 3, 4, 5, 6])  # create instance attribute
+
+    @property
+    def MYCONFIG(self):
+        return self._MYCONFIG
+
+```
+where `MYCONFIG` is the name of this particular configuration. Define an instance attribute holding the joint configuration, it must be a
+NumPy array with N elements.  Then define a property that will return that attribute.
+
+Many of the
+Toolbox models have a configuration called `qz` which is the set of zero
+joint angles.
+
+Finally, we can make the robot definition an executable script.  We do that by adding a hashbang line at the top of the file
+
+```python
+#!/usr/bin/env python
+```
+
+and a main code block at the bottom
+
+```python
+if __name__ == '__main__':
+
+    robot = MYROBOT()
+    print(robot)
+```
+
+so if you run it from your shell
+
+```
+% ./Puma560.py 
+┏━━━━━━━━━┳━━━━━━━━━┳━━━━━━━━┳━━━━━━━┓
+┃   θⱼ    ┃   dⱼ    ┃   aⱼ   ┃  ⍺ⱼ   ┃
+┣━━━━━━━━━╋━━━━━━━━━╋━━━━━━━━╋━━━━━━━┫
+┃q0 + 0.0 ┃   0.672 ┃      0 ┃  90.0 ┃
+┃q1 + 0.0 ┃       0 ┃ 0.4318 ┃   0.0 ┃
+┃q2 + 0.0 ┃ 0.15005 ┃ 0.0203 ┃ -90.0 ┃
+┃q3 + 0.0 ┃  0.4318 ┃      0 ┃  90.0 ┃
+┃q4 + 0.0 ┃       0 ┃      0 ┃ -90.0 ┃
+┃q5 + 0.0 ┃       0 ┃      0 ┃   0.0 ┃
+┗━━━━━━━━━┻━━━━━━━━━┻━━━━━━━━┻━━━━━━━┛
+```
+
+we see the table of Denavit-Hartenberg parameters.
+
+### Adding your model to the Toolbox
+
+Edit the file `__init__.py` in this folder.  Add a line like:
+
+```python
+from roboticstoolbox.models.DH.Stanford import MYROBOT
+```
+
+and then add `'MYROBOT'` to the list that defines `__all__`.
+
+### Testing
+
+If you made your definition an executable Python script as above, then run it
+and check that the parameters are what you expect them to be.
+
+If you defined some configurations you can also test that they are correct.
+
+```python
+    myrobot = MYROBOT()  # instantiate an instance of your model
+    print(myrobot)       # display its kinematic parameters
+    print(myrobot.MYCONFIG)    # check that the joint configuration works
+```
+
+### Next steps
+
+You can now use the power of the Toolbox to compute forward and inverse
+kinematics, display a graphical model, and interactively teach the robot.
+If you defined dynamic parameters then you can compute forward and inverse
+rigid-body dyamics and simulate the response of the robot to applied torques.
+
+Good luck and enjoy!
+
+### Contribute your model
+
+If you think your model might be interesting to others consider submitting a pull request.

roboticstoolbox/models/DH/Sawyer.py

@@ -0,0 +1,85 @@
+#!/usr/bin/env python
+"""
+@author: Peter Corke
+"""
+
+import numpy as np
+from roboticstoolbox import DHRobot, RevoluteDH
+
+
+class Sawyer(DHRobot):
+    """
+    Class that models a Sawyer manipulator
+
+    :param symbolic: use symbolic constants
+    :type symbolic: bool
+
+    ``Sawyer()`` is an object which models a Rethink Sawyer robot and
+    describes its kinematic characteristics using standard DH
+    conventions.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.DH.Sawyer()
+        >>> print(robot)
+
+    Defined joint configurations are:
+
+    - qz, zero joint angle configuration, 'L' shaped configuration
+
+    :references:
+
+        -`Layeghi, Daniel. “Dynamic and Kinematic Modelling of the Sawyer Arm ” Google Sites, 20 Nov. 2017 <https://sites.google.com/site/daniellayeghi/daily-work-and-writing/major-project-2>`_
+
+    .. note:: SI units of metres are used.
+
+    """
+
+    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
+        mm = 1e-3
+
+        # kinematic parameters
+        a = np.r_[81, 0, 0, 0, 0, 0, 0] * mm
+        d = np.r_[317, 192.5, 400, 168.5, 400, 136.3, 133.75] * mm
+        alpha = [-pi / 2, -pi / 2, -pi / 2, -pi / 2, -pi / 2, -pi / 2, 0]
+
+        links = []
+
+        for j in range(7):
+            link = RevoluteDH(d=d[j], a=a[j], alpha=alpha[j])
+            links.append(link)
+
+        super().__init__(
+            links,
+            name="Sawyer",
+            manufacturer="Rethink Robotics",
+            keywords=(
+                "redundant",
+                "symbolic",
+            ),
+            symbolic=symbolic,
+        )
+
+        self.qr = np.zeros(7)
+        self.qz = np.zeros(7)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+
+if __name__ == "__main__":  # pragma nocover
+    sawyer = Sawyer(symbolic=False)
+    print(sawyer)