roboticstoolbox-python 1.3.0__cp312-cp312-win_amd64.whl

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)

roboticstoolbox/models/DH/Stanford.py

@@ -0,0 +1,147 @@
+#!/usr/bin/env python
+"""
+@author: Peter Corke
+"""
+
+from roboticstoolbox import DHRobot, RevoluteDH, PrismaticDH
+from math import pi
+import numpy as np
+
+
+class Stanford(DHRobot):
+    """
+    Class that models a Stanford arm manipulator
+
+    ``Stanford()`` is a class 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.Stanford()
+        >>> print(robot)
+
+    Defined joint configurations are:
+
+        - qz, zero joint angle configuration
+
+    .. note::
+        - SI units are used.
+        - Gear ratios not currently known, though reflected armature inertia
+          is known, so gear ratios are set to 1.
+
+    :references:
+        - Kinematic data from "Modelling, Trajectory calculation and Servoing
+          of a computer controlled arm".  Stanford AIM-177.  Figure 2.3
+        - Dynamic data from "Robot manipulators: mathematics, programming and
+          control"
+          Paul 1981, Tables 6.5, 6.6
+        - Dobrotin & Scheinman, "Design of a computer controlled manipulator
+          for robot research", IJCAI, 1973.
+
+    .. codeauthor:: Peter Corke
+
+    """
+
+    def __init__(self):
+
+        deg = pi / 180
+        inch = 0.0254
+
+        L0 = RevoluteDH(
+            d=0.412,  # link length (Dennavit-Hartenberg notation)
+            a=0,  # link offset (Dennavit-Hartenberg notation)
+            alpha=-pi / 2,  # link twist (Dennavit-Hartenberg notation)
+            # inertia tensor of link with respect to
+            # center of mass I = [L_xx, L_yy, L_zz,
+            # L_xy, L_yz, L_xz]
+            I=[0.276, 0.255, 0.071, 0, 0, 0],
+            # distance of ith origin to center of mass [x,y,z]
+            # in link reference frame
+            r=[0, 0.0175, -0.1105],
+            m=9.29,  # mass of link
+            Jm=0.953,  # actuator inertia
+            G=1,  # gear ratio
+            qlim=[-170 * deg, 170 * deg],
+        )  # minimum and maximum joint angle
+
+        L1 = RevoluteDH(
+            d=0.154,
+            a=0.0,
+            alpha=pi / 2,
+            I=[0.108, 0.018, 0.100, 0, 0, 0],
+            r=[0, -1.054, 0],
+            m=5.01,
+            Jm=2.193,
+            G=1,
+            qlim=[-170 * deg, 170 * deg],
+        )
+
+        L2 = PrismaticDH(
+            theta=-pi / 2,
+            a=0.0203,
+            alpha=0,
+            I=[2.51, 2.51, 0.006, 0, 0, 0],
+            r=[0, 0, -6.447],
+            m=4.25,
+            Jm=0.782,
+            G=1,
+            qlim=[12 * inch, (12 + 38) * inch],
+        )
+
+        L3 = RevoluteDH(
+            d=0,
+            a=0,
+            alpha=-pi / 2,
+            I=[0.002, 0.001, 0.001, 0, 0, 0],
+            r=[0, 0.092, -0.054],
+            m=1.08,
+            Jm=0.106,
+            G=1,
+            qlim=[-170 * deg, 170 * deg],
+        )
+
+        L4 = RevoluteDH(
+            d=0,
+            a=0,
+            alpha=pi / 2,
+            I=[0.003, 0.0004, 0, 0, 0, 0],
+            r=[0, 0.566, 0.003],
+            m=0.630,
+            Jm=0.097,
+            G=1,
+            qlim=[-90 * deg, 90 * deg],
+        )
+
+        L5 = RevoluteDH(
+            d=0,
+            a=0,
+            alpha=0,
+            I=[0.013, 0.013, 0.0003, 0, 0, 0],
+            r=[0, 0, 1.554],
+            m=0.51,
+            Jm=0.020,
+            G=1,
+            qlim=[-170 * deg, 170 * deg],
+        )
+
+        L = [L0, L1, L2, L3, L4, L5]
+
+        super().__init__(
+            L,
+            name="Stanford arm",
+            manufacturer="Victor Scheinman",
+            keywords=("dynamics",),
+        )
+
+        self.qr = np.zeros(6)
+        self.qz = np.zeros(6)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+
+if __name__ == "__main__":  # pragma nocover
+    stanford = Stanford()
+    print(stanford)

roboticstoolbox/models/DH/TwoLink.py

@@ -0,0 +1,109 @@
+"""
+@author: Peter Corke
+"""
+
+from roboticstoolbox import DHRobot, RevoluteDH
+
+# from math import pi
+from spatialmath import SE3
+import numpy as np
+
+
+class TwoLink(DHRobot):
+    """
+    Class that models a 2-link robot moving in the vertical plane
+
+    :param symbolic: use symbolic constants
+    :type symbolic: bool
+
+    ``TwoLink()`` is a class which models a 2-link planar robot and
+    describes its kinematic and dynamic characteristics using standard DH
+    conventions. of a simple planar 2-link mechanism moving in the xz-plane, it experiences gravity loading.
+    All mass is concentrated at the joints.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.DH.TwoLink()
+        >>> print(robot)
+
+    The parameters values depend on the ``symbolic`` parameter
+
+    =======================================  =================  ==============
+    Parameters                               Numeric values     Symbolic values
+    =======================================  =================  ==============
+    link lengths                             1, 1               a1, a2
+    link masses                              1, 1               m1, m2
+    link CoMs in the link frame x-direction  -0.5, -0.5         c1, c2
+    gravitational acceleration               9.8                g
+    =======================================  =================  ==============
+
+    Defined joint configurations are:
+
+        - qz, zero angles, all folded up
+        - q1, links are horizontal and vertical respectively
+        - q2, links are vertical and horizontal respectively
+        - qn, nominal working configuration
+
+    .. note::
+
+        - Robot has only 2 DoF.
+        - Motor inertia is 0.
+        - Link inertias are 0.
+        - Viscous and Coulomb friction is 0.
+
+    :Reference: Based on Fig 3-6 (p73) of Spong and Vidyasagar (1st edition).
+
+    .. 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
+            m1, m2 = sym.symbol("m1 m2")  # type: ignore
+            c1, c2 = sym.symbol("c1 c2")  # type: ignore
+            g = sym.symbol("g")
+        else:
+            from math import pi
+
+            zero = 0.0
+            a1 = 1
+            a2 = 1
+            m1 = 1
+            m2 = 1
+            c1 = -0.5
+            c2 = -0.5
+            g = 9.8
+
+        links = [
+            RevoluteDH(a=a1, alpha=zero, m=m1, r=[c1, 0, 0]),
+            RevoluteDH(a=a2, alpha=zero, m=m2, r=[c2, 0, 0]),
+        ]
+
+        super().__init__(
+            links, symbolic=symbolic, name="2 link", keywords=("planar", "dynamics")
+        )
+
+        self.qr = np.array([pi / 6, -pi / 6])
+        self.qz = np.zeros(2)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+        self.addconfiguration_attr("qz", [0, 0])
+        self.addconfiguration_attr("q1", [0, pi / 2])
+        self.addconfiguration_attr("q2", [pi / 2, -pi / 2])
+        self.addconfiguration_attr("qn", [pi / 6, -pi / 6])
+
+        self.base = SE3.Rx(pi / 2)
+        self.gravity = [0, 0, g]
+
+
+if __name__ == "__main__":  # pragma nocover
+    robot = TwoLink(symbolic=True)
+    print(robot)

roboticstoolbox/models/DH/UR10.py

@@ -0,0 +1,124 @@
+import numpy as np
+from roboticstoolbox import DHRobot, RevoluteDH
+from spatialmath import SE3
+
+
+class UR10(DHRobot):
+    """
+    Class that models a Universal Robotics UR10 manipulator
+
+    :param symbolic: use symbolic constants
+    :type symbolic: bool
+
+    ``UR10()`` 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.UR10()
+        >>> print(robot)
+
+    Defined joint configurations are:
+
+    - qz, zero joint angle configuration
+    - qr, arm horizontal along x-axis
+
+    .. note::
+        - SI units are used.
+
+    :References:
+
+        - `Parameters for calculations of kinematics and dynamics <https://www.universal-robots.com/articles/ur/parameters-for-calculations-of-kinematics-and-dynamics>`_
+
+    :sealso: :func:`UR3`, :func:`UR5`
+
+
+    .. 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
+
+        # robot length values (metres)
+        a = [0, -0.612, -0.5723, 0, 0, 0]
+        d = [0.1273, 0, 0, 0.163941, 0.1157, 0.0922]
+
+        alpha = [pi / 2, zero, zero, pi / 2, -pi / 2, zero]
+
+        # mass data
+        mass = [7.1, 12.7, 4.27, 2.000, 2.000, 0.365]
+        center_of_mass = [
+            [0.021, 0, 0.027],
+            [0.38, 0, 0.158],
+            [0.24, 0, 0.068],
+            [0.0, 0.007, 0.018],
+            [0.0, 0.007, 0.018],
+            [0, 0, -0.026],
+        ]
+
+        # inertia matrices for each link
+        inertia = [
+            np.array(
+                [[0.0341, 0, -0.0043], [0, 0.0353, 0.0001], [-0.0043, 0.0001, 0.0216]]
+            ),
+            np.array(
+                [[0.0281, 0.0001, -0.0156], [0.0001, 0.7707, 0], [-0.0156, 0, 0.7694]]
+            ),
+            np.array(
+                [[0.0101, 0.0001, 0.0092], [0.0001, 0.3093, 0], [0.0092, 0, 0.3065]]
+            ),
+            np.array(
+                [[0.0030, -0.0000, 0], [-0.0000, 0.0022, -0.0002], [0, -0.0002, 0.0026]]
+            ),
+            np.array(
+                [[0.0030, -0.0000, 0], [-0.0000, 0.0022, -0.0002], [0, -0.0002, 0.0026]]
+            ),
+            np.array([[0, 0, 0], [0, 0.0004, 0], [0, 0, 0.0003]]),
+        ]
+
+        links = []
+
+        for j in range(6):
+            link = RevoluteDH(
+                d=d[j],
+                a=a[j],
+                alpha=alpha[j],
+                m=mass[j],
+                r=center_of_mass[j],
+                G=1,
+                I=inertia[j],
+            )
+            links.append(link)
+
+        super().__init__(
+            links,
+            name="UR10",
+            manufacturer="Universal Robotics",
+            keywords=("dynamics", "symbolic"),
+            symbolic=symbolic,
+        )
+
+        self.qr = np.array([180, 0, 0, 0, 90, 0]) * deg
+        self.qz = np.zeros(6)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+
+if __name__ == "__main__":  # pragma nocover
+    ur10 = UR10(symbolic=False)
+    print(ur10)
+    # print(ur10.dyntable())