Merge pull request #805 from compas-dev/cfg_import

Fix missing import on RobotModel

Author: gonzalocasas

CHANGELOG.md

@@ -21,6 +21,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 * Fixed box scaling.
 * Fixed a bug in `Polyline.divide_polyline_by_length` related to a floating point rounding error.
+* Fixed bug in `RobotModel.zero_configuration`.
 
 ### Removed
 

src/compas/robots/configuration.py

@@ -5,13 +5,20 @@
 from math import pi
 
 from compas.base import Base
-from compas.robots.model.joint import Joint
 
 __all__ = [
     'Configuration',
     'FixedLengthList',
 ]
 
+# These joint types known to configuration are a match
+# to the ones defined in `Joint` class, but we redefine here
+# to avoid a circular dependency
+_JOINT_REVOLUTE = 0
+_JOINT_CONTINUOUS = 1
+_JOINT_PRISMATIC = 2
+_JOINT_PLANAR = 5
+
 
 def joint_names_validator(joint_names, key=None, value=None):
     new_joint_names = list(joint_names)
@@ -266,7 +273,7 @@ def from_revolute_values(cls, values, joint_names=None):
         """
         values = list(values)
         joint_names = list(joint_names or [])
-        return cls.from_data({'joint_values': values, 'joint_types': [Joint.REVOLUTE] * len(values), 'joint_names': joint_names})
+        return cls.from_data({'joint_values': values, 'joint_types': [_JOINT_REVOLUTE] * len(values), 'joint_names': joint_names})
 
     @classmethod
     def from_prismatic_and_revolute_values(cls, prismatic_values, revolute_values, joint_names=None):
@@ -291,8 +298,8 @@ def from_prismatic_and_revolute_values(cls, prismatic_values, revolute_values, j
         revolute_values = list(revolute_values)
         joint_names = list(joint_names or [])
         values = prismatic_values + revolute_values
-        joint_types = [Joint.PRISMATIC] * \
-            len(prismatic_values) + [Joint.REVOLUTE] * len(revolute_values)
+        joint_types = [_JOINT_PRISMATIC] * \
+            len(prismatic_values) + [_JOINT_REVOLUTE] * len(revolute_values)
         return cls.from_data({'joint_values': values, 'joint_types': joint_types, 'joint_names': joint_names})
 
     @classmethod
@@ -353,12 +360,12 @@ def prismatic_values(self):
 
         E.g. positions on the external axis system.
         """
-        return [v for i, v in enumerate(self.joint_values) if self.joint_types[i] == Joint.PRISMATIC]
+        return [v for i, v in enumerate(self.joint_values) if self.joint_types[i] == _JOINT_PRISMATIC]
 
     @property
     def revolute_values(self):
         """:obj:`list` of :obj:`float` : Revolute joint values in radians."""
-        return [v for i, v in enumerate(self.joint_values) if self.joint_types[i] == Joint.REVOLUTE]
+        return [v for i, v in enumerate(self.joint_values) if self.joint_types[i] == _JOINT_REVOLUTE]
 
     def copy(self):
         """Create a copy of this :class:`Configuration`.
@@ -388,7 +395,7 @@ def scale(self, scale_factor):
         values_scaled = []
 
         for value, joint_type in zip(self.joint_values, self.joint_types):
-            if joint_type in (Joint.PLANAR, Joint.PRISMATIC):
+            if joint_type in (_JOINT_PLANAR, _JOINT_PRISMATIC):
                 value *= scale_factor
             values_scaled.append(value)
 
@@ -457,7 +464,7 @@ def iter_differences(self, other):
 
         for i, (v1, v2) in enumerate(value_pairs):
             diff = v1 - v2
-            if self.joint_types[i] in [Joint.REVOLUTE, Joint.CONTINUOUS]:
+            if self.joint_types[i] in [_JOINT_REVOLUTE, _JOINT_CONTINUOUS]:
                 d1 = diff % (2 * pi)
                 d1 = d1 if diff >= 0 else d1 - 2*pi
                 d2 = d1 - 2*pi if diff >= 0 else d1 + 2*pi

src/compas/robots/model/robot.py

@@ -9,18 +9,19 @@
 from compas.base import Base
 from compas.datastructures import Mesh
 from compas.files import URDF
-from compas.files import URDFParser
 from compas.files import URDFElement
+from compas.files import URDFParser
 from compas.geometry import Frame
 from compas.geometry import Transformation
+from compas.robots import Configuration
 from compas.robots.model.geometry import Color
 from compas.robots.model.geometry import Geometry
 from compas.robots.model.geometry import Material
 from compas.robots.model.geometry import MeshDescriptor
 from compas.robots.model.geometry import Origin
 from compas.robots.model.geometry import Texture
-from compas.robots.model.geometry import _attr_to_data
 from compas.robots.model.geometry import _attr_from_data
+from compas.robots.model.geometry import _attr_to_data
 from compas.robots.model.joint import Axis
 from compas.robots.model.joint import Joint
 from compas.robots.model.joint import Limit
@@ -30,7 +31,6 @@
 from compas.robots.resources import DefaultMeshLoader
 from compas.topology import shortest_path
 
-
 __all__ = ['RobotModel']
 
 
@@ -1085,21 +1085,3 @@ def remove_joint(self, name):
 URDFParser.install_parser(Material, 'robot/material')
 URDFParser.install_parser(Color, 'robot/material/color')
 URDFParser.install_parser(Texture, 'robot/material/texture')
-
-
-if __name__ == '__main__':
-    import os
-    import doctest
-    from compas import HERE
-    from compas.geometry import Sphere  # noqa: F401
-    from compas.robots import GithubPackageMeshLoader, Configuration  # noqa: F401
-
-    ur5_urdf_file = os.path.join(HERE, '..', '..', 'tests', 'compas', 'robots', 'fixtures', 'ur5.xacro')
-
-    robot = RobotModel("robot", links=[], joints=[])
-    link0 = robot.add_link("world")
-    link1 = robot.add_link("link1")
-    link2 = robot.add_link("link2")
-    robot.add_joint("joint1", Joint.CONTINUOUS, link0, link1, Frame.worldXY(), (0, 0, 1))
-    robot.add_joint("joint2", Joint.CONTINUOUS, link1, link2, Frame.worldXY(), (0, 0, 1))
-    doctest.testmod(globs=globals())

tests/compas/robots/test_model.py

@@ -1,5 +1,5 @@
-import re
 import os
+import re
 
 import pytest
 
@@ -126,6 +126,12 @@ def test_ur5_urdf(ur5_file):
     assert len(list(filter(lambda i: i.type == Joint.REVOLUTE, r.joints))) == 6
 
 
+def test_zero_configuration(ur5_file):
+    r = RobotModel.from_urdf_file(ur5_file)
+    assert r.zero_configuration().joint_values == [0.0] * 6
+    assert len(list(filter(lambda i: i.type == Joint.REVOLUTE, r.joints))) == 6
+
+
 def test_ur5_urdf_to_string(ur5_file):
     r_original = RobotModel.from_urdf_file(ur5_file)
     urdf = URDF.from_robot(r_original)
@@ -625,8 +631,9 @@ def test_ensure_geometry(urdf_file, urdf_file_with_shapes_only):
     import os
     from zipfile import ZipFile
     try:
-        from StringIO import StringIO as ReaderIO
         from urllib import urlopen
+
+        from StringIO import StringIO as ReaderIO
     except ImportError:
         from io import BytesIO as ReaderIO
         from urllib.request import urlopen