def test_errors():
"""
Test error.
:return:
"""
with raises(PyboticsError):
raise PyboticsError()
assert str(PyboticsError()) == PyboticsError().message
assert str(PyboticsError("test")) == "test"
def test_errors():
"""
Test error.
:return:
"""
with raises(PyboticsError):
raise PyboticsError()
assert str(PyboticsError()) is PyboticsError._default_message
assert str(PyboticsError('test')) is 'test'
def vector_2_matrix(
vector: Sequence[float],
convention: Union[OrientationConvention,
str] = OrientationConvention.EULER_ZYX,
) -> np.ndarray:
"""
Calculate the pose from the position and euler angles.
:param convention:
:param vector: transform vector
:return: 4x4 transform matrix
"""
# get individual variables
translation_component = vector[:3]
rotation_component = vector[-3:]
# validate and extract orientation info
if isinstance(convention, OrientationConvention):
convention = convention.value
try:
OrientationConvention(convention)
except ValueError as e:
raise PyboticsError(str(e))
# iterate through rotation order
# build rotation matrix
transform_matrix = np.eye(4)
for axis, value in zip(convention, rotation_component): # type: ignore
current_rotation = globals()[f"rotation_matrix_{axis}"](value)
transform_matrix = np.dot(transform_matrix, current_rotation)
# add translation component
transform_matrix[:-1, -1] = translation_component
return transform_matrix
def _validate_transform_mask(mask: Union[bool, Sequence[bool]], name: str,
size: int) -> Sequence[bool]:
"""Validate mask arguments."""
# validate input
if isinstance(mask, bool):
return [mask] * size
elif len(mask) != size:
raise PyboticsError(f"{name} must be of length {size}")
else:
return mask
def translation_matrix(xyz: Sequence[float]) -> np.ndarray:
"""Generate a basic 4x4 translation matrix."""
# validate
if len(xyz) != 3:
raise PyboticsError("len(xyz) must be 3")
matrix = np.eye(4)
matrix[:-1, -1] = xyz
return matrix
def translation_matrix(xyz: Sequence[float]) -> np.ndarray:
"""Generate a basic 4x4 translation matrix."""
# validate
if len(xyz) != POSITION_VECTOR_LENGTH:
raise PyboticsError(
'len(xyz) must be {}'.format(POSITION_VECTOR_LENGTH))
matrix = np.eye(4)
matrix[:-1, -1] = xyz
return matrix
def _validate_links(value: Union[Sequence[MDHLink], np.ndarray]) -> Sequence[MDHLink]:
if isinstance(value, np.ndarray):
try:
value = value.reshape((-1, MDHLink._size))
except ValueError as e:
logger.error(str(e))
raise PyboticsError(f"MDH links have {MDHLink.size} parameters per link.")
# FIXME: only assumes revolute joints
value = [RevoluteMDHLink(*x) for x in value]
return value
def _validate_transform_mask(mask: Union[bool, Sequence[bool]], name: str,
required_length: int) -> Sequence[bool]:
"""Validate mask arguments."""
# validate input
if isinstance(mask, bool):
return [mask] * required_length
elif len(mask) != required_length:
raise PyboticsError('{} must be of length {}'.format(
name, required_length))
else:
return mask
def position_limits(self, value: np.ndarray) -> None:
if value.shape[0] != 2 or value.shape[1] != len(self):
raise PyboticsError(
'position_limits must have shape=(2,{})'.format(len(self)))
self._position_limits = value
def joint_limits(self, value: np.ndarray) -> None:
"""Set joint limits."""
if value.shape[0] != 2 or value.shape[1] != len(self):
raise PyboticsError(f"position_limits must have shape=(2,{len(self)})")
self._joint_limits = value
def joints(self, value: np.ndarray) -> None:
"""Set joints."""
if np.any(value < self.joint_limits[0]) or np.any(value > self.joint_limits[1]):
raise PyboticsError("Joint limits exceeded.")
self._joints = value
def matrix(self, value: np.ndarray) -> None:
if not is_4x4_matrix(value):
raise PyboticsError('4x4 transform matrix is required.')
self._matrix = value
def cg(self, value: Sequence[float]) -> None:
if not is_vector(value, POSITION_VECTOR_LENGTH):
raise PyboticsError('CG must be 1D vector if length {}.'.format(
POSITION_VECTOR_LENGTH))
self._cg = np.array(value)
