fixed r urdf

jhavl · jhavl · commit 8fcb4576c1ff · 2020-10-26T12:29:11.000+10:00
diff --git a/examples/test.py b/examples/test.py
@@ -0,0 +1,23 @@
+#!/usr/bin/env python
+"""
+@author Jesse Haviland
+"""
+
+import roboticstoolbox as rtb
+import spatialmath as sm
+import numpy as np
+import qpsolvers as qp
+import time
+
+# Launch the simulator Swift
+env = rtb.backend.Swift()
+env.launch()
+
+# Create a Panda robot object
+puma = rtb.models.Puma560()
+
+# Set joint angles to ready configuration
+puma.q = puma.qr
+
+# Add the puma to the simulator
+env.add(puma)
diff --git a/roboticstoolbox/backend/Swift/Swift.py b/roboticstoolbox/backend/Swift/Swift.py
@@ -38,7 +38,7 @@ class Swift(Connector):  # pragma nocover
     :references:
 
         - https://github.com/jhavl/swift
-        
+
     """
     def __init__(self):
         super(Swift, self).__init__()
@@ -72,11 +72,11 @@ def step(self, dt=50):
 
         :param dt: time step in milliseconds, defaults to 50
         :type dt: int, optional
- 
+
         ``env.step(args)`` triggers an update of the 3D scene in the Swift
         window referenced by ``env``.
 
-        .. note:: 
+        .. note::
 
             - Each robot in the scene is updated based on
               their control type (position, velocity, acceleration, or torque).
@@ -89,7 +89,7 @@ def step(self, dt=50):
         """
 
         # TODO how is the pose of shapes updated prior to step?
-        
+
         super().step
 
         self._step_robots(dt)
diff --git a/roboticstoolbox/backend/urdf/urdf.py b/roboticstoolbox/backend/urdf/urdf.py
@@ -1744,7 +1744,7 @@ def __init__(self, name, links, joints=None,
         # Store the visuals, collisions, and inertials
         for i in range(len(joints)):
             link = self._link_map[joints[i].child]
-            elinks[i].r = link.inertial.origin
+            elinks[i].r = link.inertial.origin[:3, 3]
             elinks[i].m = link.inertial.mass
             elinks[i].inertia = link.inertial.inertia
 
diff --git a/roboticstoolbox/robot/DHDynamics.py b/roboticstoolbox/robot/DHDynamics.py
@@ -1427,7 +1427,7 @@ def _cross(a, b):
         a[0] * b[1] - a[1] * b[0]]
 
 
-if __name__ == "__main__":
+if __name__ == "__main__":   # pragma nocover
 
     import roboticstoolbox as rtb
     # from spatialmath.base import symbolic as sym
diff --git a/roboticstoolbox/robot/ERobot.py b/roboticstoolbox/robot/ERobot.py
@@ -265,7 +265,7 @@ def urdf_to_ets_args(self, file_path, tld=None):
         # get the path to the class that defines the robot
         classpath = sys.modules[self.__module__].__file__
         # add on relative path to get to the URDF or xacro file
-        base_path = PurePath(classpath).parent.parent / 'xacro' 
+        base_path = PurePath(classpath).parent.parent / 'xacro'
         file_path = base_path / PurePosixPath(file_path)
         name, ext = splitext(file_path)
 
diff --git a/roboticstoolbox/robot/Link.py b/roboticstoolbox/robot/Link.py
@@ -4,6 +4,7 @@
 from spatialmath.base.argcheck import getvector, \
     isscalar, isvector, ismatrix
 
+
 def _listen_dyn(func):
     @wraps(func)
     def wrapper_listen_dyn(*args):
@@ -12,9 +13,11 @@ def wrapper_listen_dyn(*args):
         return func(*args)
     return wrapper_listen_dyn
 
+
 class Link:
 
-    def __init__(self,             
+    def __init__(
+            self,
             name='',
             offset=0.0,
             qlim=np.zeros(2),
@@ -49,7 +52,6 @@ def __init__(self,
         self.Tc = Tc
         self.G = G
 
-
     def copy(self):
         """
         Copy of link object
@@ -278,7 +280,7 @@ def flip(self):
         - ``link.flip = ...`` checks and sets the joint flip status
 
         Joint flip defines the direction of motion of the joint.
-        
+
         ``flip = False`` is conventional motion direction:
 
             - revolute motion is a positive rotation about the z-axis
@@ -350,12 +352,12 @@ def I(self):    # noqa
             :rtype: ndarray(3,3)
         - ``link.I = ...`` checks and sets the link inertia
 
-        Link inertia is a symmetric 3x3 matrix describing the inertia with respect
-        to a frame with its origin at the centre of mass, and with axes parallel to those of
-        the link frame.
+        Link inertia is a symmetric 3x3 matrix describing the inertia with
+        respect to a frame with its origin at the centre of mass, and with
+        axes parallel to those of the link frame.
 
         The inertia matrix is
-        
+
         .. math::
 
             \begin{bmatrix} I_{xx} & I_{xy} & I_{xz} \\
@@ -435,7 +437,8 @@ def B(self):
             :rtype: float
         - ``link.B = ...`` checks and sets the motor viscous friction
 
-        .. note:: Viscous friction is the same for positive and negative motion.
+        .. note:: Viscous friction is the same for positive and negative
+            motion.
         """
         return self._B
 
@@ -455,7 +458,7 @@ def Tc(self):
         Get/set motor Coulomb friction
 
         - ``link.Tc`` is the motor Coulomb friction
-            :return: motor Coulomb friction 
+            :return: motor Coulomb friction
             :rtype: ndarray(2)
         - ``link.Tc = ...`` checks and sets the motor Coulomb friction. If a
           scalar is given the value is set to [T, -T], if a 2-vector it is
@@ -470,7 +473,8 @@ def Tc(self):
                 \tau_C^+ & \mbox{if $\dot{q} > 0$} \\
                 \tau_C^- & \mbox{if $\dot{q} < 0$} \end{array} \right.
 
-        .. note:: :math:`\tau_C^+` must be :math:`> 0`, and :math:`\tau_C^-` must be :math:`< 0`.
+        .. note:: :math:`\tau_C^+` must be :math:`> 0`, and :math:`\tau_C^-`
+            must be :math:`< 0`.
         """
         return self._Tc
 
@@ -513,4 +517,3 @@ def G(self):
     @_listen_dyn
     def G(self, G_new):
         self._G = G_new
-
diff --git a/tests/test_ELink.py b/tests/test_ELink.py
@@ -160,7 +160,7 @@ def test_properties(self):
         l0 = rp.ELink()
 
         self.assertEqual(l0.m, 0.0)
-        nt.assert_array_almost_equal(l0.r.A, np.eye(4))
+        nt.assert_array_almost_equal(l0.r, np.zeros(3))
         self.assertEqual(l0.Jm, 0.0)
 
     def test_fail_parent(self):
