Z1 Robot arm from Unitree

roboticstoolbox/models/URDF/Z1.py

@@ -0,0 +1,74 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.robot.ERobot import ERobot
+from math import pi
+
+
+class Z1(ERobot):
+    """
+    Class that imports a Z1 URDF model
+
+    ``Z1()`` is a class which imports a Unitree Z1 robot
+    definition from a URDF file.  The model describes its kinematic and
+    graphical characteristics.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.URDF.Z1()
+        >>> print(robot)
+
+    Defined joint configurations are:
+
+    - qz, zero joint angle configuration, 'L' shaped configuration
+    - qr, vertical 'READY' configuration
+
+    .. codeauthor:: Keith Siilats
+    .. sectionauthor:: Peter Corke
+    """
+
+    def __init__(self):
+
+        links, name, urdf_string, urdf_filepath = self.URDF_read(
+                      "z1_description/xacro/robot.xacro"
+        )
+        # for link in links:
+        #     print(link)
+
+        super().__init__(
+            links,
+            name=name.upper(),
+            manufacturer="Unitree",
+            # gripper_links=links[9],
+            urdf_string=urdf_string,
+            urdf_filepath=urdf_filepath,
+        )
+        # forward, -0.000019, 1.542859, -1.037883, -0.531308, 0.002487, -0.012173, 0.999650, -0.002146, -0.026357, 0.389527, 0.002468, 0.999923, 0.012173, 0.000269, 0.026329, -0.012234, 0.999578, 0.402549, 0.000000, 0.000000, 0.000000, 1.000000,
+        # start, -0.000336, 0.001634, 0.000000, 0.064640, 0.000248, 0.000230, 0.997805, 0.000104, 0.066225, 0.048696, -0.000087, 1.000000, -0.000252, 0.000011, -0.066225, 0.000246, 0.997805, 0.148729, 0.000000, 0.000000, 0.000000, 1.000000,
+
+        self.qr = np.array([0.000922, 0.000680, -0.003654, -0.075006, -0.000130, 0.000035,0])
+        self.qz = np.array([0.000922, 0.000680, -0.003654, -0.075006, -0.000130, 0.000035,0])
+        self.grab3 = np.array([-0.247,1.271, -1.613, -0.267, -0.617,0.916,0])
+
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+        self.addconfiguration("grab3", self.grab3)
+
+        # sol=robot.ikine_LM(SE3(0.5, -0.2, 0.2)@SE3.OA([1,0,0],[0,0,-1]))
+        # 0, 0.000922, 0.000680, -0.003654, -0.075006, -0.000130, 0.000035, 0.000006, -0.002146, -0.002523, -0.003688, -0.002988, -0.000048, 0.001385, 0.016346,
+        self.addconfiguration_attr(
+            "qn",
+            np.array(
+                [0.000922, 0.000680, -0.003654, -0.075006, -0.000130, 0.000035,0]
+            ),
+        )
+        self.addconfiguration_attr("q1", [0.000922, 0.000680, -0.003654, -0.075006, -0.000130, 0.000035,0])
+
+
+if __name__ == "__main__":  # pragma nocover
+
+    robot = Z1()
+    print(robot)
+    print(robot.ets())

roboticstoolbox/models/URDF/__init__.py

@@ -23,6 +23,7 @@
 from roboticstoolbox.models.URDF.FetchCamera import FetchCamera
 from roboticstoolbox.models.URDF.Valkyrie import Valkyrie
 from roboticstoolbox.models.URDF.AL5D import AL5D
+from roboticstoolbox.models.URDF.Z1 import Z1
 
 __all__ = [
     "Panda",
@@ -50,4 +51,5 @@
     "FetchCamera",
     "Valkyrie",
     "AL5D",
+    "Z1"
 ]

roboticstoolbox/robot/Robot.py

@@ -1817,7 +1817,8 @@ def rne(
 # ================= Robot2 Class ============================================== #
 # ============================================================================= #
 
-
+#if link != self._base_link: #keith needed this for z1 in ERobot somewhere
+
 class Robot2(BaseRobot[Link2]):
     def __init__(self, arg, **kwargs):
         if isinstance(arg, ETS2):

roboticstoolbox/tools/xacro/__init__.py

@@ -927,7 +927,10 @@ def eval_all(node, macros, symbols):
 
         # TODO: Also evaluate content of COMMENT_NODEs?
         elif node.nodeType == xml.dom.Node.TEXT_NODE:
-            node.data = unicode(eval_text(node.data, symbols))
+            try:
+                node.data = unicode(eval_text(node.data, symbols))
+            except AttributeError as e:
+                raise
 
         node = next
 
@@ -1067,24 +1070,24 @@ def main(filename, tld_other=None):   # pragma: no cover
     else:
         tld = tld_other
 
-    try:
-        # open and process file
-        doc = process_file(filename, **opts)
-        # open the output file
-        out = open_output(opts["output"])
-
-    except Exception as e:
-        msg = unicode(e)
-        if not msg:
-            msg = repr(e)
-        error(msg)
-        if verbosity > 0:
-            print_location(filestack, e)
-        if verbosity > 1:
-            print(file=sys.stderr)  # add empty separator line before error
-            raise  # create stack trace
-        else:
-            sys.exit(2)  # gracefully exit with error condition
+    # try:
+    # open and process file
+    doc = process_file(filename, **opts)
+    # open the output file
+    out = open_output(opts["output"])
+
+    # except Exception as e:
+    #     msg = unicode(e)
+    #     if not msg:
+    #         msg = repr(e)
+    #     error(msg)
+    #     if verbosity > 0:
+    #         print_location(filestack, e)
+    #     if verbosity > 1:
+    #         print(file=sys.stderr)  # add empty separator line before error
+    #         raise  # create stack trace
+    #     else:
+    #         sys.exit(2)  # gracefully exit with error condition
 
     # special output mode
     if opts["just_deps"]: