def transforms(self, position: Optional[Sequence[float]] = None) -> \
Sequence[np.ndarray]:
"""
Generate a sequence of spatial transforms.
The sequence represents the given position of the kinematic chain.
:param position: given position of kinematic chain
:return: sequence of transforms
"""
# validate
if position is not None:
if not is_1d_sequence(position, self.num_dof):
raise SequenceError('position', self.num_dof)
else:
position = np.zeros(len(self))
# FIXME: remove type ignore for mypy bugs:
# error: Argument 2 to "zip" has incompatible type
# "Optional[Iterable[float]]"; expected "Iterable[float]";
# error: Call to untyped function "transform"
# of "Link" in typed context
transforms = [
link.transform(p) for link, p in zip(self.links, position)
] # type: ignore
return transforms
def fk(self, position: Optional[Sequence[float]] = None) -> np.ndarray:
"""
Compute the forward kinematics of a given position.
Uses the current position if None is given.
:param position:
:return: 4x4 transform matrix of the FK pose
"""
# validate
if position is not None:
if not is_1d_sequence(position, self.num_dof):
raise SequenceError('position', self.num_dof)
else:
position = self.position
# gather transforms
transforms = [self.world_frame.matrix]
transforms.extend(self.kinematic_chain.transforms(position))
transforms.append(self.tool.matrix)
# matrix multiply through transforms
pose = np.eye(4)
for t in transforms:
pose = np.dot(pose, t)
return pose
def euler_zyx_2_matrix(vector: Sequence[float]) -> np.ndarray:
"""
Calculate the pose from the position and euler angles.
:param vector: transform vector
:return: 4x4 transform matrix
"""
# validate input
if not is_1d_sequence(vector, TRANSFORM_VECTOR_LENGTH):
raise SequenceError('vector', TRANSFORM_VECTOR_LENGTH)
# get individual variables
x, y, z, a, b, c = vector
# get trig values
ca = np.cos(a)
sa = np.sin(a)
cb = np.cos(b)
sb = np.sin(b)
cc = np.cos(c)
sc = np.sin(c)
# get resulting transform
transform = [[cb * cc, -cb * sc, sb, x],
[ca * sc + cc * sa * sb, ca * cc - sa * sb * sc, -cb * sa, y],
[sa * sc - ca * cc * sb, cc * sa + ca * sb * sc, ca * cb, z],
[0, 0, 0, 1]]
return np.array(transform, dtype=np.float)
def array_2_links(
array: np.ndarray,
link_convention: LinkConvention = LinkConvention.MDH,
kinematic_pairs: Union[
KinematicPair, Sequence[KinematicPair]] = KinematicPair.REVOLUTE
) -> Sequence[Link]:
"""
Generate a sequence of links from a given array of link parameters.
:param kinematic_pairs:
:param array: link parameters
:param link_convention: link convention
:return: sequence of links
"""
# validate
if link_convention in LinkConvention:
# vectors are reshaped to a 2D array based
# on number of parameters per link
array = array.reshape((-1, link_convention.value))
# validate
if isinstance(kinematic_pairs, KinematicPair):
# turn single KinematicPair into sequence
kinematic_pairs = [kinematic_pairs] * len(array)
else:
if not is_1d_sequence(kinematic_pairs, len(array)):
raise SequenceError('kinematic_pairs', len(array))
if not all(
[isinstance(kp, KinematicPair) for kp in kinematic_pairs]):
raise KinematicPairError()
# create link sequences based on convention;
links = []
# TODO: add `if link_convention is LinkConvention.MDH:` check
for row, _ in zip(array, kinematic_pairs):
# TODO: add `if kp is KinematicPair.REVOLUTE:` check
links.append(RevoluteMDHLink(*row))
else:
raise LinkConventionError()
return links
def test_is_1d_sequence():
"""
Test validation function.
:return:
"""
# good, usual case
x = np.eye(1, 3).ravel()
assert is_1d_sequence(x)
assert is_1d_sequence(x.tolist())
assert is_1d_sequence(x, 3)
assert is_1d_sequence(x.tolist(), 3)
# bad, almost 1D array
x = np.eye(1, 3)
assert not is_1d_sequence(x)
assert not is_1d_sequence(x.tolist())
# test types
assert not is_1d_sequence([1.2, 2.3], sequence_type=int)
def position(self, value: Sequence[float]) -> None:
if is_1d_sequence(value, POSITION_VECTOR_LENGTH):
self.matrix[:-1, -1] = value
else:
raise SequenceError('value', POSITION_VECTOR_LENGTH)
def cg(self, value: Sequence[float]) -> None:
if is_1d_sequence(value, POSITION_VECTOR_LENGTH):
self._cg = np.array(value)
else:
raise SequenceError('value', POSITION_VECTOR_LENGTH)