def update_gait(self):
'''
Method that updates the gait with input from gait translation and rotation vectors.
Updates gait transformation matrix and places virtual potential field charges.
'''
if self.gait_translation is None and self.gait_rotation is None:
self.logger.error('Cannot update Gait Transformation matrix when both translation and rotations vectors are None.')
return
if self.gait_translation is None:
self.gait_transform_matrix_new = self.hexapod.body.get_t_matrix() * kinematics.get_t_matrix(numpy.matrix([[0.0], [0.0], [0.0], [1.0]]), self.gait_rotation) * self.hexapod.body.get_i_t_matrix()
elif self.gait_rotation is None:
self.gait_transform_matrix_new = self.hexapod.body.get_t_matrix() * kinematics.get_t_matrix(self.gait_translation, numpy.matrix([[0.0], [0.0], [0.0]])) * self.hexapod.body.get_i_t_matrix()
else:
self.gait_transform_matrix_new = self.hexapod.body.get_t_matrix() * kinematics.get_t_matrix(self.gait_translation, self.gait_rotation) * self.hexapod.body.get_i_t_matrix()
if self.gait_transform_matrix is None:
self.gait_transform_matrix = self.gait_transform_matrix_new
def start_next_tripod_cycle(self):
"""
cycle method for tripod gait
"""
#self.logger.info('Cycle: %d', self.cycle)
self.ground_feet, self.free_feet = self.free_feet, self.ground_feet
if (self.gait_transform_matrix != self.gait_transform_matrix_new).any() and self.cycle % 2 == 0:
self.gait_transform_matrix = self.gait_transform_matrix_new
cycle_gait_t_matrix = self.gait_transform_matrix
#self.logger.info('Gait T Matrix:\n%s\nNew Gait T Matrix:\n%s', self.gait_transform_matrix, self.gait_transform_matrix_new)
#self.logger.info('Gait T Matrix:\n%s', cycle_gait_t_matrix)
if self.cycle == 0:
for foot in self.free_feet:
self.feet_start[foot] = self.feet_pos[foot]
self.feet_dest[foot] = (self.hexapod.body.get_t_matrix() * kinematics.get_t_matrix(self.gait_translation / 2, self.gait_rotation / 2) * self.hexapod.body.get_i_t_matrix() * numpy.append(self.feet_start[foot], numpy.matrix([1]), axis=0))[0:3]
else:
for foot in self.free_feet:
self.feet_start[foot] = self.feet_pos[foot]
self.feet_dest[foot] = (cycle_gait_t_matrix * numpy.append(self.feet_start[foot], numpy.matrix([1]), axis=0))[0:3]