Add unit conversion

src/compas_rrc/common.py

@@ -17,12 +17,18 @@
 CLIENT_PROTOCOL_VERSION = 2
 
 
-def _convert_unit(value, type_):
+def _convert_unit_to_meters_radians(value, type_):
     if type_ in {Joint.REVOLUTE, Joint.CONTINUOUS}:
         return math.radians(value)
     return value / 1000
 
 
+def _convert_unit_to_mm_degrees(value, type_):
+    if type_ in {Joint.REVOLUTE, Joint.CONTINUOUS}:
+        return math.degrees(value)
+    return value * 1000
+
+
 class FeedbackLevel(object):
     """Represents default valid feedback levels.
 
@@ -203,7 +209,7 @@ def to_configuration_primitive(self, joint_types, joint_names=None):
         -------
         :class:`compas.robots.Configuration`
         """
-        joint_values = [_convert_unit(value, type_) for value, type_ in zip(self.values, joint_types)]
+        joint_values = [_convert_unit_to_meters_radians(value, type_) for value, type_ in zip(self.values, joint_types)]
         return Configuration(joint_values, joint_types, joint_names)
 
     def to_configuration(self, robot, group=None):
@@ -242,8 +248,10 @@ def from_configuration_primitive(cls, configuration, joint_names=None):
         :class:`compas_rrc.ExternalAxes`
         """
         if joint_names:
-            return cls(configuration[name] for name in joint_names)
-        return cls(configuration.joint_values)
+            joint_values = [_convert_unit_to_mm_degrees(configuration[name], configuration.type_dict[name]) for name in joint_names]
+        else:
+            joint_values = [_convert_unit_to_mm_degrees(value, type_) for value, type_ in zip(configuration.joint_values, configuration.joint_types)]
+        return cls(joint_values)
 
     @classmethod
     def from_configuration(cls, configuration, robot=None, group=None):
@@ -359,7 +367,7 @@ def to_configuration_primitive(self, joint_types, joint_names=None):
         -------
         :class:`compas.robots.Configuration`
         """
-        joint_values = [_convert_unit(value, type_) for value, type_ in zip(self.values, joint_types)]
+        joint_values = [_convert_unit_to_meters_radians(value, type_) for value, type_ in zip(self.values, joint_types)]
         return Configuration(joint_values, joint_types, joint_names)
 
     def to_configuration(self, robot, group=None):
@@ -398,8 +406,10 @@ def from_configuration_primitive(cls, configuration, joint_names=None):
         :class:`compas_rrc.RobotJoints`
         """
         if joint_names:
-            return cls(configuration[name] for name in joint_names)
-        return cls(configuration.joint_values)
+            joint_values = [_convert_unit_to_mm_degrees(configuration[name], configuration.type_dict[name]) for name in joint_names]
+        else:
+            joint_values = [_convert_unit_to_mm_degrees(value, type_) for value, type_ in zip(configuration.joint_values, configuration.joint_types)]
+        return cls(joint_values)
 
     @classmethod
     def from_configuration(cls, configuration, robot=None, group=None):

tests/test_common.py

@@ -1,19 +1,29 @@
 import math
 
+from compas.geometry import allclose
 from compas.robots import Configuration
 import compas_rrc as rrc
 
 
 def test__convert_units():
-    v = rrc.common._convert_unit(180, 0)
+    v = rrc.common._convert_unit_to_meters_radians(180, 0)
     assert v == math.pi
 
-    v = rrc.common._convert_unit(-180, 1)
+    v = rrc.common._convert_unit_to_meters_radians(-180, 1)
     assert v == -math.pi
 
-    v = rrc.common._convert_unit(2545, 2)
+    v = rrc.common._convert_unit_to_meters_radians(2545, 2)
     assert v == 2.545
 
+    v = rrc.common._convert_unit_to_mm_degrees(math.pi, 0)
+    assert v == 180
+
+    v = rrc.common._convert_unit_to_mm_degrees(-math.pi, 1)
+    assert v == -180
+
+    v = rrc.common._convert_unit_to_mm_degrees(2.545, 2)
+    assert v == 2545
+
 
 def test_robot_joints():
     j = rrc.RobotJoints()
@@ -41,15 +51,21 @@ def test_robot_joints():
     assert c.joint_values == [math.pi/6, -math.pi/4, 0.1]
     assert len(c.joint_names) == 0
 
-    config = Configuration([0, 1, 2, 3, 4, 5], [0, 0, 0, 0, 0, 0])
+    config = Configuration([2*math.pi, math.pi, math.pi/2, math.pi/3, math.pi/4, math.pi/6], [0, 0, 0, 0, 0, 0])
     rj = rrc.RobotJoints.from_configuration_primitive(config)
-    assert rj.rax_1 == 0
-    assert rj.rax_6 == 5
+    assert allclose(rj.values, [360, 180, 90, 60, 45, 30])
 
-    config = Configuration([0, 1, 2, 3, 4, 5, 6], [0, 0, 0, 0, 0, 0, 0], ['a', 'b', 'c', 'd', 'e', 'f', 'g'])
+    config = Configuration(
+        [0, 2*math.pi, math.pi, math.pi/2, math.pi/3, math.pi/4, math.pi/6 ],
+        [0, 0, 0, 0, 0, 0, 0],
+        ['a', 'b', 'c', 'd', 'e', 'f', 'g'])
     rj = rrc.RobotJoints.from_configuration_primitive(config, ['b', 'c', 'd', 'e', 'f', 'g'])
-    assert rj.rax_1 == 1
-    assert rj.rax_6 == 6
+    assert allclose(rj.values, [360, 180, 90, 60, 45, 30])
+
+    j = rrc.RobotJoints(30, 10, 0)
+    config = j.to_configuration_primitive([0, 0, 0])
+    new_j = rrc.RobotJoints.from_configuration_primitive(config)
+    assert allclose(j.values, new_j.values)
 
 
 def test_external_axes():
@@ -78,12 +94,18 @@ def test_external_axes():
     assert c.joint_values == [math.pi/6, -math.pi/4, 0.1]
     assert len(c.joint_names) == 0
 
-    config = Configuration([0, 1, 2, 3, 4, 5], [0, 0, 0, 0, 0, 0])
+    config = Configuration([2*math.pi, math.pi, math.pi/2, math.pi/3, math.pi/4, math.pi/6], [0, 0, 0, 0, 0, 0])
     rj = rrc.ExternalAxes.from_configuration_primitive(config)
-    assert rj.eax_a == 0
-    assert rj.eax_f == 5
+    assert allclose(rj.values, [360, 180, 90, 60, 45, 30])
 
-    config = Configuration([0, 1, 2, 3, 4, 5, 6], [0, 0, 0, 0, 0, 0, 0], ['a', 'b', 'c', 'd', 'e', 'f', 'g'])
+    config = Configuration(
+        [0, 2*math.pi, math.pi, math.pi/2, math.pi/3, math.pi/4, math.pi/6 ],
+        [0, 0, 0, 0, 0, 0, 0],
+        ['a', 'b', 'c', 'd', 'e', 'f', 'g'])
     rj = rrc.ExternalAxes.from_configuration_primitive(config, ['b', 'c', 'd', 'e', 'f', 'g'])
-    assert rj.eax_a == 1
-    assert rj.eax_f == 6
+    assert allclose(rj.values, [360, 180, 90, 60, 45, 30])
+
+    j = rrc.RobotJoints(30, 10, 0)
+    config = j.to_configuration_primitive([0, 0, 0])
+    new_j = rrc.ExternalAxes.from_configuration_primitive(config)
+    assert allclose(j.values, new_j.values)