Skip to content

Commit

Permalink
Update usage to COMPAS 2.x
Browse files Browse the repository at this point in the history
  • Loading branch information
@gonzalocasas
gonzalocasas committed Mar 27, 2024
1 parent 9f4d18a commit 030d681
Show file tree
Hide file tree
Showing 2 changed files with 97 additions and 52 deletions.
103 changes: 68 additions & 35 deletions src/compas_rrc/common.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,28 +5,30 @@
from compas_robots import Configuration
from compas_robots.model import Joint

__all__ = ['CLIENT_PROTOCOL_VERSION',
'FeedbackLevel',
'ExecutionLevel',
'InstructionException',
'TimeoutException',
'FutureResult',
'ExternalAxes',
'RobotJoints']
__all__ = [
"CLIENT_PROTOCOL_VERSION",
"FeedbackLevel",
"ExecutionLevel",
"InstructionException",
"TimeoutException",
"FutureResult",
"ExternalAxes",
"RobotJoints",
]

CLIENT_PROTOCOL_VERSION = 2


def _convert_unit_to_meters_radians(value, type_):
if type_ in {Joint.REVOLUTE, Joint.CONTINUOUS}:
return math.radians(value)
return value / 1000.
return value / 1000.0


def _convert_unit_to_mm_degrees(value, type_):
if type_ in {Joint.REVOLUTE, Joint.CONTINUOUS}:
return math.degrees(value)
return value * 1000.
return value * 1000.0


class FeedbackLevel(object):
Expand All @@ -35,6 +37,7 @@ class FeedbackLevel(object):
.. autoattribute:: NONE
.. autoattribute:: DONE
"""

NONE = 0
"""Indicates no feedback is requested from the robot."""

Expand All @@ -50,6 +53,7 @@ class ExecutionLevel(object):
.. autoattribute:: ROBOT
.. autoattribute:: CONTROLLER
"""

ROBOT = 0
"""Execute instruction on the robot task."""

Expand All @@ -61,12 +65,15 @@ class InstructionException(Exception):
"""Exception caused during/after the execution of an instruction."""

def __init__(self, message, result):
super(InstructionException, self).__init__('{}, RRC Reply={}'.format(message, result))
super(InstructionException, self).__init__(
"{}, RRC Reply={}".format(message, result)
)
self.result = result


class TimeoutException(Exception):
"""Timeout exception caused during execution of an instruction."""

pass


Expand All @@ -91,7 +98,7 @@ def result(self, timeout=None):
"""
if not self.done:
if not self.event.wait(timeout):
raise TimeoutException('Timeout: future result not available')
raise TimeoutException("Timeout: future result not available")

if isinstance(self.value, Exception):
raise self.value
Expand Down Expand Up @@ -177,7 +184,7 @@ def eax_f(self, value):

# List accessors
def __repr__(self):
return 'ExternalAxes({})'.format([round(i, 2) for i in self.values])
return "ExternalAxes({})".format([round(i, 2) for i in self.values])

def __len__(self):
return len(self.values)
Expand Down Expand Up @@ -208,13 +215,16 @@ def to_configuration_primitive(self, joint_types, joint_names=None):
Returns
-------
:class:`compas.robots.Configuration`
:class:`compas_robots.Configuration`
"""
joint_values = [_convert_unit_to_meters_radians(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):
"""Convert the ExternalAxes to a :class:`compas.robots.Configuration`, including the unit conversion
"""Convert the ExternalAxes to a :class:`compas_robots.Configuration`, including the unit conversion
from mm and degrees to meters and radians.
Parameters
Expand All @@ -227,20 +237,20 @@ def to_configuration(self, robot, group=None):
Returns
-------
:class:`compas.robots.Configuration`
:class:`compas_robots.Configuration`
"""
joint_types = robot.get_configurable_joint_types(group)
joint_names = robot.get_configurable_joint_names(group)
return self.to_configuration_primitive(joint_types, joint_names)

@classmethod
def from_configuration_primitive(cls, configuration, joint_names=None):
"""Create an instance of ``ExternalAxes`` from a :class:`compas.robots.Configuration`, including the unit
"""Create an instance of ``ExternalAxes`` from a :class:`compas_robots.Configuration`, including the unit
conversion from meters and radians to mm and degrees.
Parameters
----------
configuration : :class:`compas.robots.Configuration`
configuration : :class:`compas_robots.Configuration`
The configuration from which to create the ``ExternalAxes`` instance.
joint_names : :obj:`list`
An optional list of joint names from the ``configuration`` whose corresponding
Expand All @@ -251,19 +261,29 @@ def from_configuration_primitive(cls, configuration, joint_names=None):
:class:`compas_rrc.ExternalAxes`
"""
if joint_names:
joint_values = [_convert_unit_to_mm_degrees(configuration[name], configuration.type_dict[name]) for name in joint_names]
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)]
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):
"""Create an instance of ``ExternalAxes`` from a :class:`compas.robots.Configuration`, including the unit
"""Create an instance of ``ExternalAxes`` from a :class:`compas_robots.Configuration`, including the unit
conversion from meters and radians to mm and degrees.
Parameters
----------
configuration : :class:`compas.robots.Configuration`
configuration : :class:`compas_robots.Configuration`
The configuration from which to create the ``ExternalAxes`` instance.
robot : :class:`compas_fab.robots.Robot`
The robot to be configured. Optional.
Expand Down Expand Up @@ -339,7 +359,7 @@ def rax_6(self, value):

# List accessors
def __repr__(self):
return 'RobotJoints({})'.format([round(i, 2) for i in self.values])
return "RobotJoints({})".format([round(i, 2) for i in self.values])

def __len__(self):
return len(self.values)
Expand All @@ -358,7 +378,7 @@ def __iter__(self):

# Conversion methods
def to_configuration_primitive(self, joint_types, joint_names=None):
"""Convert the RobotJoints to a :class:`compas.robots.Configuration`, including the unit conversion
"""Convert the RobotJoints to a :class:`compas_robots.Configuration`, including the unit conversion
from mm and degrees to meters and radians.
Parameters
Expand All @@ -370,13 +390,16 @@ def to_configuration_primitive(self, joint_types, joint_names=None):
Returns
-------
:class:`compas.robots.Configuration`
:class:`compas_robots.Configuration`
"""
joint_values = [_convert_unit_to_meters_radians(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):
"""Convert the RobotJoints to a :class:`compas.robots.Configuration`, including the unit conversion
"""Convert the RobotJoints to a :class:`compas_robots.Configuration`, including the unit conversion
from mm and degrees to meters and radians.
Parameters
Expand All @@ -389,20 +412,20 @@ def to_configuration(self, robot, group=None):
Returns
-------
:class:`compas.robots.Configuration`
:class:`compas_robots.Configuration`
"""
joint_types = robot.get_configurable_joint_types(group)
joint_names = robot.get_configurable_joint_names(group)
return self.to_configuration_primitive(joint_types, joint_names)

@classmethod
def from_configuration_primitive(cls, configuration, joint_names=None):
"""Create an instance of ``RobotJoints`` from a :class:`compas.robots.Configuration`, including the unit
"""Create an instance of ``RobotJoints`` from a :class:`compas_robots.Configuration`, including the unit
conversion from meters and radians to mm and degrees.
Parameters
----------
configuration : :class:`compas.robots.Configuration`
configuration : :class:`compas_robots.Configuration`
The configuration from which to create the ``RobotJoints`` instance.
joint_names : :obj:`list`
An optional list of joint names from the ``configuration`` whose corresponding
Expand All @@ -413,19 +436,29 @@ def from_configuration_primitive(cls, configuration, joint_names=None):
:class:`compas_rrc.RobotJoints`
"""
if joint_names:
joint_values = [_convert_unit_to_mm_degrees(configuration[name], configuration.type_dict[name]) for name in joint_names]
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)]
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):
"""Create an instance of ``RobotJoints`` from a :class:`compas.robots.Configuration`, including the unit
"""Create an instance of ``RobotJoints`` from a :class:`compas_robots.Configuration`, including the unit
conversion from meters and radians to mm and degrees.
Parameters
----------
configuration : :class:`compas.robots.Configuration`
configuration : :class:`compas_robots.Configuration`
The configuration from which to create the ``ExternalAxes`` instance.
robot : :class:`compas_fab.robots.Robot`
The robot to be configured. Optional.
Expand Down
46 changes: 29 additions & 17 deletions tests/test_common.py
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
import math

from compas.geometry import allclose
from compas.robots import Configuration
from compas_robots import Configuration
import compas_rrc as rrc


Expand Down Expand Up @@ -42,24 +42,30 @@ def test_robot_joints():
assert list(j) == [30, 10, 0]

j = rrc.RobotJoints(30, 45, 0)
c = j.to_configuration_primitive([0, 1, 2], ['j1', 'j2', 'j3'])
assert c.joint_values == [math.pi/6, math.pi/4, 0.0]
assert c.joint_names == ['j1', 'j2', 'j3']
c = j.to_configuration_primitive([0, 1, 2], ["j1", "j2", "j3"])
assert c.joint_values == [math.pi / 6, math.pi / 4, 0.0]
assert c.joint_names == ["j1", "j2", "j3"]

j = rrc.RobotJoints(30, -45, 100)
c = j.to_configuration_primitive([0, 1, 2])
assert c.joint_values == [math.pi/6, -math.pi/4, 0.1]
assert c.joint_values == [math.pi / 6, -math.pi / 4, 0.1]
assert len(c.joint_names) == 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])
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 allclose(rj.values, [360, 180, 90, 60, 45, 30])

config = Configuration(
[0, 2*math.pi, math.pi, math.pi/2, math.pi/3, math.pi/4, math.pi/6 ],
[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'])
["a", "b", "c", "d", "e", "f", "g"],
)
rj = rrc.RobotJoints.from_configuration_primitive(
config, ["b", "c", "d", "e", "f", "g"]
)
assert allclose(rj.values, [360, 180, 90, 60, 45, 30])

j = rrc.RobotJoints(30, 10, 0)
Expand All @@ -85,24 +91,30 @@ def test_external_axes():
assert list(ea) == [30, 10, 0]

j = rrc.ExternalAxes(30, 45, 0)
c = j.to_configuration_primitive([0, 1, 2], ['j1', 'j2', 'j3'])
assert c.joint_values == [math.pi/6, math.pi/4, 0.0]
assert c.joint_names == ['j1', 'j2', 'j3']
c = j.to_configuration_primitive([0, 1, 2], ["j1", "j2", "j3"])
assert c.joint_values == [math.pi / 6, math.pi / 4, 0.0]
assert c.joint_names == ["j1", "j2", "j3"]

j = rrc.ExternalAxes(30, -45, 100)
c = j.to_configuration_primitive([0, 1, 2])
assert c.joint_values == [math.pi/6, -math.pi/4, 0.1]
assert c.joint_values == [math.pi / 6, -math.pi / 4, 0.1]
assert len(c.joint_names) == 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])
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 allclose(rj.values, [360, 180, 90, 60, 45, 30])

config = Configuration(
[0, 2*math.pi, math.pi, math.pi/2, math.pi/3, math.pi/4, math.pi/6 ],
[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'])
["a", "b", "c", "d", "e", "f", "g"],
)
rj = rrc.ExternalAxes.from_configuration_primitive(
config, ["b", "c", "d", "e", "f", "g"]
)
assert allclose(rj.values, [360, 180, 90, 60, 45, 30])

j = rrc.RobotJoints(30, 10, 0)
Expand Down

0 comments on commit 030d681

Please sign in to comment.