def set_new_orientation(self, orientation):
self.matrix = tf.quaternion_matrix(orientation)
self.matrix[:3, 3] = self.position
self.matrix = np.dot(self.matrix, self.mat_trans)
orientation = tf.quaternion_from_matrix(self.matrix)
position = self.matrix[:3, 3]
self.set_orientation(orientation)
self.set_position(position)
def set_new_orientation(self, orientation):
self.matrix = tf.quaternion_matrix(orientation)
self.matrix[:3, 3] = self.position
self.matrix = np.dot(self.matrix, self.mat_trans)
orientation = tf.quaternion_from_matrix(self.matrix)
position = self.matrix[:3, 3]
self.set_orientation(orientation)
self.set_position(position)
def __init__(self, length):
ShapeMarker.__init__(self)
self.marker.type = self.marker.ARROW
self.set_length(length)
self.matrix = np.eye(4)
arrow_pos0 = [0, 0, length]
mat_inicio = tf.translation_matrix(arrow_pos0)
quat = [0, np.sin(np.deg2rad(45)), 0, np.cos(np.deg2rad(45))]
matrix_pos_0 = tf.quaternion_matrix(quat)
self.mat_trans = np.dot(mat_inicio, matrix_pos_0)
def __init__(self, length):
ShapeMarker.__init__(self)
self.marker.type = self.marker.ARROW
self.set_length(length)
self.matrix = np.eye(4)
arrow_pos0 = [0, 0, length]
mat_inicio = tf.translation_matrix(arrow_pos0)
quat = [0, np.sin(np.deg2rad(45)),
0, np.cos(np.deg2rad(45))]
matrix_pos_0 = tf.quaternion_matrix(quat)
self.mat_trans = np.dot(mat_inicio, matrix_pos_0)