def _apply_geometric_transformations(self, theta, symbolic):
if symbolic:
# Angle symbolique qui paramètre la rotation du joint en cours
symbolic_frame_matrix = np.eye(4)
# Apply translation matrix
symbolic_frame_matrix = symbolic_frame_matrix * sympy.Matrix(geometry.homogeneous_translation_matrix(*self.translation_vector))
# Apply orientation matrix
symbolic_frame_matrix = symbolic_frame_matrix * geometry.cartesian_to_homogeneous(geometry.rpy_matrix(*self.orientation))
# Apply rotation matrix
if self.rotation is not None:
symbolic_frame_matrix = symbolic_frame_matrix * geometry.cartesian_to_homogeneous(geometry.symbolic_axis_rotation_matrix(self.rotation, theta), matrix_type="sympy")
symbolic_frame_matrix = sympy.lambdify(theta, symbolic_frame_matrix, "numpy")
return symbolic_frame_matrix
else:
# Init the transformation matrix
frame_matrix = np.eye(4)
# First, apply translation matrix
frame_matrix = np.dot(frame_matrix, geometry.homogeneous_translation_matrix(*self.translation_vector))
# Apply orientation
frame_matrix = np.dot(frame_matrix, geometry.cartesian_to_homogeneous(geometry.rpy_matrix(*self.orientation)))
# Apply rotation matrix
if self.rotation is not None:
frame_matrix = np.dot(frame_matrix, geometry.cartesian_to_homogeneous(geometry.axis_rotation_matrix(self.rotation, theta)))
return frame_matrix
def get_rotation_axis(self):
if self.rotation is not None:
return np.dot(
geometry.homogeneous_translation_matrix(*self.translation_vector),
np.dot(
geometry.cartesian_to_homogeneous(geometry.rpy_matrix(*self.orientation)),
geometry.cartesian_to_homogeneous_vectors(self.rotation * self.axis_length)
)
)
else:
raise ValueError("This link doesn't provide a rotation")
