def vector(self) -> np.ndarray:
"""
Return the vector representation of the frame as EULER ZYX.
:return: vectorized frame
"""
return matrix_2_vector(self.matrix)
def apply_optimization_vector(self, vector: np.ndarray) -> None:
"""Apply vector."""
# get number of parameters
num_kc_parameters = np.sum(self.kinematic_chain_mask)
num_tool_parameters = np.sum(self.tool_mask)
# extract vector segments
segments = np.split(
vector,
[num_kc_parameters, num_kc_parameters + num_tool_parameters])
kc_segment = segments[0]
tool_segment = segments[1]
world_segment = segments[2]
# update vectors
kc_vector = self.robot.kinematic_chain.vector
kc_vector[self.kinematic_chain_mask] = kc_segment
self.robot.kinematic_chain.vector = kc_vector
tool_vector = self.robot.tool.vector
tool_vector[self.tool_mask] = tool_segment
self.robot.tool.vector = tool_vector
world_vector = matrix_2_vector(self.robot.world_frame)
world_vector[self.world_mask] = world_segment
self.robot.world_frame = vector_2_matrix(world_vector)
def generate_optimization_vector(self) -> np.ndarray:
"""Generate vector."""
kc_vector = np.compress(self.kinematic_chain_mask,
self.robot.kinematic_chain.vector)
tool_vector = np.compress(self.tool_mask, self.robot.tool.vector)
world_vector = np.compress(self.world_mask,
matrix_2_vector(self.robot.world_frame))
return np.hstack((kc_vector, tool_vector, world_vector))
def test_tool():
"""Test."""
tool = Tool()
cg = [1, 2, 3]
tool.cg = cg
np.testing.assert_allclose(tool.cg, cg)
p = [1, 2, 3]
tool.position = p
np.testing.assert_allclose(tool.position, p)
np.testing.assert_allclose(tool.vector, matrix_2_vector(tool.matrix))
def test_matrix_2_vector(vector_transforms: Sequence[dict]):
"""Test."""
for d in vector_transforms:
for c in [
e for e in OrientationConvention.__members__.values()
if d["order"] == e.value
]:
try:
actual_vector = matrix_2_vector(d["transform"].reshape((4, 4)),
c)
except NotImplementedError:
# TODO: implement other conversions
# don't fail for NotImplementedError
continue
np.testing.assert_allclose(actual=actual_vector,
desired=d["vector"],
atol=1e-6)
def test_tool():
"""Test."""
tool = Tool()
cg = [1, 2, 3]
tool.cg = cg
np.testing.assert_allclose(tool.cg, cg)
with raises(PyboticsError):
tool.cg = [1, 2, 3, 4]
with raises(PyboticsError):
tool.matrix = np.eye(5)
p = [1, 2, 3]
tool.position = p
np.testing.assert_allclose(tool.position, p)
np.testing.assert_allclose(tool.vector, matrix_2_vector(tool.matrix))