# joint_values.py
#
# Copyright (c) 2018, Xamla and/or its affiliates. All rights reserved.
#
# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# as published by the Free Software Foundation; either version 2
# of the License, or any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
#!/usr/bin/env python3
from functools import total_ordering
from typing import Iterable
import numpy as np
from .joint_set import JointSet
from xamlamoveit_msgs.msg import JointPathPoint, JointValuesPoint
[docs]class JointValues(object):
"""
Manages a set of joints and the respective joint values
Methods
-------
empty()
Creates empty instance of JointValues
zero(joint_set)
Creates instance of JointValues with all values 0.0
try_get_value(name)
Tries to get joint value by joint name
reorder(new_order)
Creates reordered instance of JointValues by the order of new_order
transform(transform_function)
Creates a transformed version of JointValues
select(names)
Creates a JointValue instance which only contains selected joints
set_values(joint_set, values)
Create a new instance of JointValues with modified values
merge(others)
Merge JointValues instance with others JointValues
from_joint_path_point_msg(joint_set, msg)
Creates an instance of JointValues from a JointPathPoint ros message
to_joint_path_point_msg()
Transform to xamlamoveit_msgs JointPathMessage
"""
def __init__(self, joint_set, values):
"""
Initialization of the JointValues class
Parameters
----------
joint_set : JoinSet
JointSet for which also the values should be managed
values : iterable[float castable]
The JointValue class can be initialized in different ways
-JointSet + only on float convertable value then all
joint values are set to the same float value
-JointSet + an iterable type with the same number of
items as number of joints in joint set. (mapping one
to one)
Returns
------
JointValues
An instance of class JointValues
Raises
------
TypeError : type mismatch
If joint_set is not of type JointSet or
type in values is not float castable
ValueError : not same size
If values is list of floats and not contains the same number
of items as joint_set or numpy array that has not the correct size
"""
self.__values = np.array([])
if not isinstance(joint_set, JointSet):
raise TypeError('joint_set is not expected type JointSet')
self.__joint_set = joint_set
try:
try:
if len(values) <= 1:
raise ValueError('max one value is provided')
except (TypeError, ValueError) as exc:
if isinstance(exc, TypeError):
values = [values] * len(self.__joint_set)
else:
values = values * len(self.__joint_set)
if len(values) != len(joint_set):
raise ValueError('joint set holds ' +
str(len(self.__joint_set)) +
' joints but only ' + str(len(values)) +
' values are provided')
self.__values = np.fromiter(values, float)
except (TypeError, ValueError) as exc:
raise exc
self.__values.flags.writeable = False
[docs] @classmethod
def from_joint_path_point_msg(cls, joint_set, msg):
"""
Creates an instance of JointValues from a JointPathPoint ros message
Parameters
----------
joint_set : JointSet
Joint set to assign joint with values
msg : JointPathPoint ros message
Message which should be transformed to JointValues
Returns
-------
JointValues
New instance of JointValues
Raises
------
TypeError
If joint_set is not of type JointSet
"""
if not isinstance(joint_set, JointSet):
raise TypeError('joint_set is not of expected type JointSet')
return cls(joint_set, msg.positions)
[docs] @classmethod
def from_joint_values_point_msg(cls, msg):
"""
Creates an instance of JointValues from JointValuesPoint message
Parameters
----------
msg : xamlamoveit_msgs JointValuesPoint message
Message which should be transformed to JointValues
Returns
-------
JointValues
New instance of JointValues
Raises
------
AttributeError
If msg not provide needed properties and methods
"""
return cls(JointSet(msg.joint_names), msg.positions)
[docs] @staticmethod
def empty():
"""
Creates a empty JointValues instance
Returns
------
joint_values : JointValues
An empty instance of JointValues
"""
joint_values = JointValues(JointSet.empty(), 0.0)
joint_values.__values = np.array([])
return joint_values
[docs] @staticmethod
def zero(joint_set):
"""
Creates instance of JointValues with all values 0.0
Parameters
----------
joint_set:
JointSet which should be managed by JointValues
Returns
------
JointValues
An JointValue instance with values initialized with 0.0
Raises
------
TypeError: type mismatch
If input parameter join_set is not of type JointSet
"""
if not isinstance(joint_set, JointSet):
raise TypeError('joint_set is not expected type JointSet')
joint_values = JointValues(joint_set, 0.0)
return joint_values
@property
def joint_set(self):
"""
joint_set : JointSet (readonly)
A instance of JointSet managing joint names
"""
return self.__joint_set
@property
def values(self):
"""
valules : numpy.ndarray(dtype=float64) (readonly)
One dimensional numpy array holding the respective
joint values
"""
return self.__values
[docs] def try_get_value(self, joint_name):
"""
Tries to get joint value by joint name
Parameters
----------
joint_name : str
joint name for which it tries to get joint value
Returns
-------
is_found : bool
If joint name exists and therefore values is found True
else False
value : float
Joint value if joint name existes else None
"""
if not isinstance(joint_name, str):
raise TypeError('joint_name expected type is str')
is_found, index = self.__joint_set.try_get_index_of(joint_name)
if not is_found:
return False, None
else:
return True, self.__values[index]
[docs] def reorder(self, new_order):
"""
Creates reordered instance of JointValue by the order of new_order
Parameters
----------
new_order : JointSet
JointSet which defines the new order
Returns
------
JointValues
A new Instance of JointValues containing the
joints in the order definied by new_order
(it is ok when new_order only holds a subset of joints)
Raises
------
TypeError : type mismatch
If input parameter new_order is not of type JointSet
ValueError :
If joint name from new_order not exists
"""
if not isinstance(new_order, JointSet):
raise TypeError('new_order is not of excpeted type JointSet')
try:
values = [self.__getitem__(joint_name) for joint_name in new_order]
except ValueError:
raise ValueError('A joint name from new_oder'
' not exist in this instance of JointValues')
return self.__class__(new_order, values)
[docs] def select(self, names):
"""
Creates a JointValue instance which only contains selected joints
Parameters
----------
names : str or list of str or JointSet
Joint names which should be in the new JointValues instance
Returns
------
JointValues
New instance of JointValues with selected joints
Raises
------
TypeError : type mismatch
If names is not of type str or list of str
ValueError :
If name not exist in joint names
"""
try:
if isinstance(names, JointSet):
return self.reorder(names)
elif isinstance(names, str):
values = self.__values[self.__joint_set.get_index_of(names)]
elif names and all(isinstance(s, str) for s in names):
values = np.zeros(len(names), self.__values.dtype)
for i, name in enumerate(names):
values[i] = self.__values[self.__joint_set.get_index_of(
name)]
else:
raise TypeError('names is not one of the expected types'
' str or list of strs')
return self.__class__(JointSet(names), values)
except ValueError as exc:
raise ValueError('name ' + name +
' not exist in joint names') from exc
[docs] def set_values(self, joint_set, values):
"""
Create a new instance of JointValues with modified values
Parameters
----------
joint_set : str, Iterable[str], JointSet
joint_set defined by single string List of strings or JointSet
values : float, Iterable[float], np.ndarray
Returns
-------
JointValues
New instance of JointValues with modified values
Raises
------
TypeError
If joint_set is not str, Iterable[str] or JointSet
If values is not iterable
ValueError
If joint_set and values unequal length
If joint defined in joint set not exist
in the JointSet of this JointValues instance
"""
if not isinstance(values, Iterable):
values = [values]
values = np.fromiter(values, float)
if not isinstance(joint_set, JointSet):
try:
joint_set = JointSet(joint_set)
except (ValueError, TypeError) as exc:
raise TypeError('joint_set is not one of expected types '
'str, Iterable[str] or JointSet') from exc
if len(joint_set) != len(values):
raise ValueError('The number of provided joints and'
' values is not equal')
m_values = self.__values.copy()
for i, joint in enumerate(joint_set):
idx = self.__joint_set.get_index_of(joint)
m_values[idx] = values[i]
return JointValues(self.__joint_set,
m_values)
[docs] def merge(self, others):
"""
Merge JointValues instance with others JointValues
Parameters
----------
others: JointValues or Iterable[JointValues]
JointValues which are merge with current instance
Returns
-------
JointValues
New instance of JointValues with contains Values for
all Joints defined in this and others JointValues instances
Raises
------
TypeError : type mismatch
If others is not one of expected types JointValues
or Iterable[JointValues]
ValueError : merge conflict
If values for some joints are defined in multiple instances of JointValues which should
be merged
"""
names_set = set(self.joint_set.names)
def check_compatability(joint_set):
nonlocal names_set
conflicts = names_set.intersection(joint_set.names)
if conflicts:
raise ValueError('merge conflict, values for'
' joints: {} are defined in multiple'
' instances of JointValues which should'
' be merged'.format(conflicts))
else:
names_set = names_set.union(joint_set.names)
if isinstance(others, JointValues):
check_compatability(others.joint_set)
joint_set = self.__joint_set.union(others.joint_set)
values = np.append(self.__values, others.values)
return self.__class__(joint_set, values)
elif (isinstance(others, Iterable) and
all(isinstance(v, JointValues) for v in others)):
for other in others:
check_compatability(other.joint_set)
joint_set = self.__joint_set.union(
list(map(lambda x: x.joint_set, others)))
values = np.append(self.__values, list(
map(lambda x: x.values, others)))
return self.__class__(joint_set, values)
else:
raise TypeError('others is not one of expected types '
'JointValues or Iterable[JointValues]')
[docs] def to_joint_path_point_msg(self):
"""
Transform to xamlamoveit_msgs JointPathPoint message
"""
joint_path_point = JointPathPoint()
joint_path_point.positions = list(self.__values)
return joint_path_point
[docs] def to_joint_values_point_msg(self):
"""
Transform to xamlamoveit_msgs JointValuesPoint message
"""
joint_values_point = JointValuesPoint()
joint_values_point.joint_names = self.__joint_set.names
joint_values_point.positions = list(self.__values)
return joint_values_point
def __getitem__(self, key):
"""
Returns value by joint name or index
Parameters
----------
key : int ,str or slice
index of joint, slice or joint name for which the values are requested
Returns
-------
value : numpy floating or numpy ndarray of floating
Joint value if index or joint name is valid / exists
Raises
------
TypeError : type mismatch
If key is not int, str or slice
ValueError :
If joint name not exists
IndexError :
If index is out of range
"""
if isinstance(key, str):
try:
return self.__values[self.__joint_set.get_index_of(key)]
except ValueError:
raise ValueError('joint name not exists')
elif isinstance(key, (int, slice)):
try:
return self.__values[key]
except IndexError:
raise IndexError('index out of range')
else:
raise TypeError(
'key is not one of expected types int, str or slice ')
def __len__(self):
return len(self.__values)
def __iter__(self):
return self.__values.__iter__()
def __str__(self):
values_str = '\n'.join([name + ' : ' + str(self.__values[i])
for i, name in enumerate(self.__joint_set)])
return 'JointValues:\n' + values_str
def __repr__(self):
return self.__str__()
def __eq__(self, other):
r_tol = 1.0e-13
a_tol = 1.0e-14
if not isinstance(other, JointValues):
return False
if id(other) == id(self):
return True
if other.joint_set != self.__joint_set:
return False
if not np.allclose(self.values, other.values,
rtol=r_tol, atol=a_tol,
equal_nan=True):
return False
return True
def __ne__(self, other):
return not self.__eq__(other)
def __add__(self, other):
if isinstance(other, self.__class__):
if len(other.joint_set) < len(self.__joint_set):
values = np.zeros(len(other.joint_set),
self.__values.dtype)
for i, name in enumerate(other.joint_set):
values[i] = (self.__values[self.__joint_set.get_index_of(name)] +
other.values[i])
return self.__class__(other.joint_set, values)
else:
values = np.zeros(
len(self.__joint_set), self.__values.dtype)
for i, name in enumerate(self.__joint_set):
values[i] = (self.__values[i] +
other.values[other.joint_set.get_index_of(name)])
else:
values = self.__values + other
return self.__class__(self.__joint_set, values)
def __radd__(self, other):
return other + self.__values
def __sub__(self, other):
if isinstance(other, self.__class__):
if len(other.joint_set) < len(self.__joint_set):
values = np.zeros(len(other.joint_set),
self.__values.dtype)
for i, name in enumerate(other.joint_set):
values[i] = (self.__values[self.__joint_set.get_index_of(name)] -
other.values[i])
return self.__class__(other.joint_set, values)
else:
values = np.zeros(
len(self.__joint_set), self.__values.dtype)
for i, name in enumerate(self.__joint_set):
values[i] = (self.__values[i] -
other.values[other.joint_set.get_index_of(name)])
else:
values = self.__values - other
return self.__class__(self.__joint_set, values)
def __rsub__(self, other):
return other - self.__values
def __mul__(self, other):
if isinstance(other, self.__class__):
if len(other.joint_set) < len(self.__joint_set):
values = np.zeros(len(other.joint_set),
self.__values.dtype)
for i, name in enumerate(other.joint_set):
values[i] = (self.__values[self.__joint_set.get_index_of(name)] *
other.values[i])
return self.__class__(other.joint_set, values)
else:
values = np.zeros(
len(self.__joint_set), self.__values.dtype)
for i, name in enumerate(self.__joint_set):
values[i] = (self.__values[i] *
other.values[other.joint_set.get_index_of(name)])
else:
values = self.__values * other
return self.__class__(self.__joint_set, values)
def __rmul__(self, other):
values = other * self.__values
return self.__class__(self.__joint_set, values)
def __truediv__(self, other):
if isinstance(other, self.__class__):
if len(other.joint_set) < len(self.__joint_set):
values = np.zeros(len(other.joint_set),
self.__values.dtype)
for i, name in enumerate(other.joint_set):
values[i] = (self.__values[self.__joint_set.get_index_of(name)] /
other.values[i])
return self.__class__(other.joint_set, values)
else:
values = np.zeros(
len(self.__joint_set), self.__values.dtype)
for i, name in enumerate(self.__joint_set):
values[i] = (self.__values[i] /
other.values[other.joint_set.get_index_of(name)])
else:
values = self.__values / other
return self.__class__(self.__joint_set, values)
def __rtruediv__(self, other):
values = other / self.__values
return self.__class__(self.__joint_set, values)