Merge branch 'master' of https://github.com/petercorke/robotics-toolbox-python

petercorke · petercorke · commit 5a305d915eab · 2020-10-26T13:25:55.000+10:00
# Conflicts:
#	roboticstoolbox/robot/Link.py
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
@@ -5,6 +5,7 @@
     isscalar, isvector, ismatrix
 from ansitable import ANSITable, Column
 
+
 def _listen_dyn(func):
     """
     Decorator for property setters
@@ -30,6 +31,7 @@ def wrapper_listen_dyn(*args):
         return func(*args)
     return wrapper_listen_dyn
 
+
 class Link:
     """
     Link superclass
@@ -67,7 +69,8 @@ class Link:
         :parts: 2
     """
 
-    def __init__(self,             
+    def __init__(
+            self,
             name='',
             offset=0.0,
             qlim=np.zeros(2),
@@ -102,7 +105,6 @@ def __init__(self,
         self.Tc = Tc
         self.G = G
 
-
     def copy(self):
         """
         Copy of link object
@@ -407,7 +409,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
@@ -485,14 +487,15 @@ def I(self):    # noqa
 
         - ``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} \\ I_{xy} & I_{yy} & I_{yz} \\I_{xz} & I_{yz} & I_{zz} \end{bmatrix}`
 
+
         and can be specified as either:
 
         - a 3 ⨉ 3 symmetric matrix
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):
