Merge pull request #287 from tassos/AL5D-support

Al5D Support

Author: jhavl

roboticstoolbox/models/DH/AL5D.py

@@ -0,0 +1,118 @@
+"""
+@author: Tassos Natsakis
+"""
+
+import numpy as np
+from roboticstoolbox import DHRobot, RevoluteMDH
+from spatialmath import SE3
+
+class AL5D(DHRobot):
+    """
+    Class that models a Lynxmotion AL5D manipulator
+
+    :param symbolic: use symbolic constants
+    :type symbolic: bool
+
+    ``AL5D()`` is an object which models a Lynxmotion AL5D robot and
+    describes its kinematic and dynamic characteristics using modified DH
+    conventions.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.DH.AL5D()
+        >>> print(robot)
+
+    Defined joint configurations are:
+
+    - qz, zero joint angle configuration
+
+    .. note::
+        - SI units are used.
+
+    :References:
+
+        - 'Reference of the robot <http://www.lynxmotion.com/c-130-al5d.aspx>'_
+
+    .. codeauthor:: Tassos Natsakis
+    """  # 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
+
+        # robot length values (metres)
+        a = [0, 0.002, 0.14679, 0.17751]
+        d = [-0.06858, 0, 0, 0]
+
+        alpha = [pi, pi/2, pi, pi]
+        offset = [pi/2, pi, -0.0427, -0.0427-pi/2]
+
+        # mass data as measured
+        mass = [0.187, 0.044, 0.207, 0.081]
+
+        # center of mass as calculated through CAD model
+        center_of_mass = [
+                [0.01724, -0.00389,  0.00468],
+                [0.07084,  0.00000,  0.00190],
+                [0.05615, -0.00251, -0.00080],
+                [0.04318,  0.00735, -0.00523],
+            ]
+
+        # moments of inertia are practically zero
+        moments_of_inertia = [
+                [0, 0, 0, 0, 0, 0],
+                [0, 0, 0, 0, 0, 0],
+                [0, 0, 0, 0, 0, 0],
+                [0, 0, 0, 0, 0, 0]
+            ]
+
+        joint_limits = [
+                [-pi/2, pi/2],
+                [-pi/2, pi/2],
+                [-pi,2, pi/2],
+                [-pi/2, pi/2],
+            ]
+
+        links = []
+
+        for j in range(4):
+            link = RevoluteMDH(
+                d=d[j],
+                a=a[j],
+                alpha=alpha[j],
+                offset=offset[j],
+                r=center_of_mass[j],
+                I=moments_of_inertia[j],
+                G=1,
+                B=0,
+                Tc=[0,0],
+                qlim=joint_limits[j]
+            )
+            links.append(link)
+            
+        tool=SE3(0.07719,0,0)
+    
+        super().__init__(
+            links,
+            name="AL5D",
+            manufacturer="Lynxmotion",
+            keywords=('dynamics', 'symbolic'),
+            symbolic=symbolic,
+            tool=tool
+        )
+    
+        # zero angles
+        self.addconfiguration("home", np.array([pi/2, pi/2, pi/2, pi/2]))
+
+if __name__ == '__main__':    # pragma nocover
+
+    al5d = AL5D(symbolic=False)
+    print(al5d)
+    print(al5d.dyntable())

roboticstoolbox/models/DH/__init__.py

@@ -21,6 +21,7 @@
 from roboticstoolbox.models.DH.TwoLink import TwoLink
 from roboticstoolbox.models.DH.Hyper3d import Hyper3d
 from roboticstoolbox.models.DH.P8 import P8
+from roboticstoolbox.models.DH.AL5D import AL5D
 
 
 __all__ = [
@@ -47,4 +48,5 @@
     'Baxter',
     'TwoLink',
     'P8',
+    'AL5D',
 ]

roboticstoolbox/models/URDF/AL5D.py

@@ -0,0 +1,53 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.robot.ERobot import ERobot
+from math import pi
+
+class AL5D(ERobot):
+    """
+    Class that imports a AL5D URDF model
+
+    ``AL5D()`` is a class which imports a Lynxmotion AL5D robot definition
+    from a URDF file.  The model describes its kinematic and graphical
+    characteristics.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.URDF.AL5D()
+        >>> print(robot)
+
+    Defined joint configurations are:
+
+    - qz, zero joint angle configuration, 'L' shaped configuration
+    - up, robot poiting upwards
+
+    .. codeauthor:: Tassos Natsakis
+    """
+
+    def __init__(self):
+
+        links, name, urdf_string, urdf_filepath = self.URDF_read(
+            "al5d_description/urdf/al5d_robot.urdf"
+        )
+
+        super().__init__(
+            links,
+            name=name,
+            urdf_string=urdf_string,
+            urdf_filepath=urdf_filepath,
+        )
+
+        self.manufacturer = "Lynxmotion"
+
+        # zero angles, upper arm pointing up, lower arm straight ahead
+        self.addconfiguration("qz", np.array([0, 0, 0, 0]))
+
+        # reference pose robot pointing upwards
+        self.addconfiguration("up", np.array([0.0000, 0.0000, 1.5707, 0.0000]))
+
+if __name__ == "__main__":  # pragma nocover
+
+    robot = AL5D()
+    print(robot)

roboticstoolbox/models/URDF/__init__.py

@@ -19,6 +19,7 @@
 from roboticstoolbox.models.URDF.LBR import LBR
 from roboticstoolbox.models.URDF.KinovaGen3 import KinovaGen3
 from roboticstoolbox.models.URDF.YuMi import YuMi
+from roboticstoolbox.models.URDF.AL5D import AL5D
 
 __all__ = [
     "Panda",
@@ -42,4 +43,5 @@
     "LBR",
     "KinovaGen3",
     "YuMi",
+    "AL5D",
 ]