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"]: