diff --git a/CHANGELOG.md b/CHANGELOG.md index 60cd4f219c3c..9c4dfc13b9ea 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/src/compas/robots/configuration.py b/src/compas/robots/configuration.py index 1370494ab583..4013b042af1e 100644 --- a/src/compas/robots/configuration.py +++ b/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 diff --git a/src/compas/robots/model/robot.py b/src/compas/robots/model/robot.py index 51f3a5a9074c..e965af7ff24c 100644 --- a/src/compas/robots/model/robot.py +++ b/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()) diff --git a/tests/compas/robots/test_model.py b/tests/compas/robots/test_model.py index f52707de0242..d941265e98e1 100644 --- a/tests/compas/robots/test_model.py +++ b/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