def rotate_body(self, angle=0, axis_vector=(0, 0, 0)):
from math import cos, sin
angle = math.radians(angle)
(l, m, n) = MathTools.normalize(*axis_vector)
rotation_matrix = numpy.array([[l * l * (1 - cos(angle)) + cos(angle),
m * l * (1 - cos(angle)) - n * sin(angle),
n * l * (1 - cos(angle)) + m * sin(angle)],
[l * m * (1 - cos(angle)) + n * sin(angle),
m * m * (1 - cos(angle)) + cos(angle),
n * m * (1 - cos(angle)) - l * sin(angle)],
[l * n * (1 - cos(angle)) - m * sin(angle),
m * n * (1 - cos(angle)) + l * sin(angle),
n * n * (1 - cos(angle)) + cos(angle)]])
bot_command = []
for l in self.legs:
bot_command.extend(l.rotate(rotation_matrix))
self._send(bot_command)
