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)