Things working again

Author: jhavl

examples/swifty.py

@@ -9,22 +9,40 @@
 import numpy as np
 import qpsolvers as qp
 
-# Launch the simulator Swift
-env = rtb.backend.Swift()
-env.launch()
+# # Launch the simulator Swift
+# env = rtb.backend.Swift()
+# env.launch()
 
 # Create a Panda robot object
-panda = rtb.models.Panda()
+panda = rtb.models.ETS.Panda()
 
-# Set joint angles to ready configuration
-panda.q = panda.qr
+print(panda)
+print(panda.base_link)
+print(panda.ee_links)
+
+path, n = panda.get_path(panda.base_link, panda.ee_links[0])
+
+q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
+panda.q = q1
+
+print(panda.fkine())
+
+for link in path:
+    print(link.name)
+
+# print(panda.get_path(panda.base_link, panda.ee_links[0])[0])
+
+# print(panda.links[5].A(0))
+
+# # Set joint angles to ready configuration
+# panda.q = panda.qr
 
 # Add the Panda to the simulator
-env.add(panda)
+# env.add(panda)
 
 
-while 1:
-    pass
+# while 1:
+#     pass
 
 
 

roboticstoolbox/backend/urdf/urdf.py

@@ -5,6 +5,7 @@
 """
 
 import numpy as np
+from spatialmath import base
 import roboticstoolbox as rtb
 import copy
 import os
@@ -1611,10 +1612,9 @@ def __init__(self, name, links, joints=None,
         # why arent the other things validated
         try:
             self._validate_transmissions()
-        except:
+        except BaseException:
             pass
 
-
         self.name = name
         self.other_xml = other_xml
 
@@ -1624,20 +1624,20 @@ def __init__(self, name, links, joints=None,
         self._transmissions = list(transmissions)
         # self._materials = list(materials)
 
-        # check for duplicate names 
-
+        # check for duplicate names
         if len(self._links) > len(set([x.name for x in self._links])):
             raise ValueError('Duplicate link names')
         if len(self._joints) > len(set([x.name for x in self._joints])):
             raise ValueError('Duplicate joint names')
-        if len(self._transmissions) > len(set([x.name for x in self._transmissions])):
-            raise ValueError('Duplicate transmission names')           
+        if len(self._transmissions) > len(
+                set([x.name for x in self._transmissions])):
+            raise ValueError('Duplicate transmission names')
 
         elinks = []
         elinkdict = {}
-        jointdict = {}
+        # jointdict = {}
 
-        ## build the list of links in URDF file order
+        # build the list of links in URDF file order
         for link in self._links:
             elink = rtb.ELink(name=link.name)
             elinks.append(elink)
@@ -1660,7 +1660,7 @@ def __init__(self, name, links, joints=None,
             except AttributeError:   # pragma nocover
                 pass
 
-        ## connect the links using joint info
+        # connect the links using joint info
         for joint in self._joints:
             # get references to joint's parent and child
             childlink = elinkdict[joint.child]
@@ -1671,12 +1671,14 @@ def __init__(self, name, links, joints=None,
 
             # constant part of link transform
             trans = sm.SE3(joint.origin).t
-            # TODO, find where reverse is used and change it to [::-1] or do that here
-            rot = joint.rpy[::-1]
+            # TODO, find where reverse is used and change
+            # it to [::-1] or do that here
+            rot = joint.rpy
             childlink._ets = rtb.ETS.SE3(trans, rot)
+            childlink._init_Ts()
 
             # variable part of link transform
-            if joint.joint_type in ('revolute', 'continuous'):   # pragma nocover
+            if joint.joint_type in ('revolute', 'continuous'):   # pragma nocover # noqa
                 if joint.axis[0] == 1:
                     var = rtb.ETS.rx()
                 elif joint.axis[0] == -1:
@@ -1826,8 +1828,6 @@ def __init__(self, name, links, joints=None,
         #     )
         #     elinks_dict[elinks[-1].name] = elinks[-1]
 
-
-
         # elinks.append(base_link)
         # self.elinks = elinks
 

roboticstoolbox/models/ETS/Panda.py

@@ -118,7 +118,8 @@ def __init__(self):
             color = ""
         else:
             color = "<<blue>>"
-        table.row(color + link.name, 
-            link.parent.name if link.parent is not None else "-", 
+        table.row(
+            color + link.name,
+            link.parent.name if link.parent is not None else "-",
             link.ets * link.v if link.v is not None else link.ets)
-    table.print()
+    table.print()

roboticstoolbox/robot/ELink.py

@@ -3,9 +3,9 @@
 @author: Jesse Haviland
 """
 
-import numpy as np
+# import numpy as np
 from spatialmath import SE3
-from spatialmath.base.argcheck import getvector, verifymatrix, isscalar
+# from spatialmath.base.argcheck import getvector, verifymatrix, isscalar
 import roboticstoolbox as rp
 from roboticstoolbox.robot.ETS import ETS
 from roboticstoolbox.robot.Link import Link
@@ -83,23 +83,9 @@ def __init__(
         self._joint_name = None
         self._jindex = None
 
-        # Number of transforms in the ETS excluding the joint variable
-        self._M = len(self._ets)
-
-        # Initialise joints
-        if isinstance(ets, ETS):
-            self._Ts = SE3()
-            for i in range(self.M):
-                if ets[i].isjoint:
-                    raise ValueError('The transforms in ets must be constant')
-
-                if not isinstance(ets[i].T(), SE3):
-                    self._Ts *= SE3(ets[i].T())
-                else:
-                    self._Ts *= ets[i].T()
-
-        elif isinstance(ets, SE3):
-            self._Ts = ets
+        # Initialise the static transform representing the constant
+        # component of the ETS
+        self._init_Ts()
 
         # Check the variable joint
         if v is None:
@@ -116,9 +102,28 @@ def __init__(
 
         self._v = v
 
+    def _init_Ts(self):
+        # Number of transforms in the ETS excluding the joint variable
+        self._M = len(self._ets)
+
+        # Initialise joints
+        if isinstance(self._ets, ETS):
+            self._Ts = SE3()
+            for i in range(self.M):
+                if self._ets[i].isjoint:
+                    raise ValueError('The transforms in ets must be constant')
+
+                if not isinstance(self._ets[i].T(), SE3):
+                    self._Ts *= SE3(self._ets[i].T())
+                else:
+                    self._Ts *= self._ets[i].T()
+
+        elif isinstance(self._ets, SE3):
+            self._Ts = self._ets
+
     def __repr__(self):
         name = self.__class__.__name__
-        s = "ets=" + str(self.ets)
+        s = "ets=" + str(self.ets())
         if self.parent is not None:
             s += ", parent=" + str(self.parent.name)
         args = [s] + super()._params()
@@ -131,12 +136,12 @@ def __str__(self):
         :return: Pretty print of the robot link
         :rtype: str
         """
-        name = self.__class__.__name__
+        # name = self.__class__.__name__
         if self.parent is None:
             parent = ""
         else:
             parent = f" [{self.parent.name}]"
-        return f"name[{self.name}({parent}): {self.ets}] "
+        return f"name[{self.name}({parent}): {self.ets()}] "
 
     @property
     def v(self):
@@ -190,7 +195,6 @@ def child(self):
     def M(self):
         return self._M
 
-
     @collision.setter
     def collision(self, coll):
         new_coll = []
@@ -272,7 +276,6 @@ def A(self, q=None, fast=False):
             else:
                 return self.Ts
 
-    @property
     def ets(self):
         if self.v is None:
             return self._ets

roboticstoolbox/robot/ERobot.py

@@ -113,6 +113,7 @@ def __init__(
                 # no, update children of this link's parent
                 link._parent._child.append(link)
 
+        for link in elinks:
             # is this a leaf node?
             if ee_links is None and len(link.child) == 0:
                 # no children, must be an end-effector
@@ -288,7 +289,7 @@ def to_dict(self):
                 'collision': []
             }
 
-            for et in link.ets:
+            for et in link.ets():
                 li['axis'].append(et.axis)
                 li['eta'].append(et.eta)
 
@@ -618,8 +619,7 @@ def fkine(self, q=None, from_link=None, to_link=None):
         if to_link is None:
             to_link = self.ee_links[0]
 
-        if q is None:
-            q = self.q
+        q = self._getq(q)
 
         path, _ = self.get_path(from_link, to_link)
         j = 0
@@ -1083,7 +1083,7 @@ def __str__(self):
             Column("parent", headalign="^"),
             Column("joint", headalign="^"),
             Column("ETS", headalign="^", colalign=">"),
-            border="thin", color=self._color)
+            border="thin")
         for k, link in enumerate(self):
             color = "" if link.isjoint else "<<blue>>"
             ee = "@" if link in self.ee_links else ""

tests/test_ERobot.py

@@ -133,11 +133,11 @@ def test_fkine_all(self):
         r2 = pm.fkine_all()
 
         for i in range(7):
-            nt.assert_array_almost_equal(p.ets[i]._fk.A, r2[i].A)
+            nt.assert_array_almost_equal(p._ets[i]._fk.A, r2[i].A)
 
         p.fkine_all(q)
         for i in range(7):
-            nt.assert_array_almost_equal(p.ets[i]._fk.A, r2[i].A)
+            nt.assert_array_almost_equal(p._ets[i]._fk.A, r2[i].A)
 
     def test_jacob0(self):
         panda = rp.models.ETS.Panda()