def update(time, leg_sensor_matrix, imu_orientation, imu_accelerations, imu_angular_rates, command): global path, state, leg_lift_height time_sources.global_time.updateTime(time) model.setSensorReadings(leg_sensor_matrix, imu_orientation, imu_angular_rates) target_angle_matrix = zeros((NUM_LEGS, LEG_DOF)) if path is None: path = BodyPause(model, controller, 1) state = STAND leg_lift_height = .2 if path.isDone(): if state == STAND: path = GoToStandardHexagon(model, controller) state = RAISE_FIRST_TRIPOD elif state == RAISE_FIRST_TRIPOD: path = TrapezoidalFeetLiftLower( model, controller, range(NUM_LEGS), [i * leg_lift_height for i in LIFT_TRIPOD_024], 2, 1) state = TURN_FIRST_TRIPOD elif state == TURN_FIRST_TRIPOD: path = RotateFeetAboutOrigin(model, controller, [0, 2, 4], TURN_ANGLE, 2, 1) state = RAISE_SECOND_TRIPOD elif state == RAISE_SECOND_TRIPOD: leg_lift_height = .4 path = TrapezoidalFeetLiftLower( model, controller, range(NUM_LEGS), [i * leg_lift_height for i in LIFT_TRIPOD_135], 2, 1) state = TURN_SECOND_TRIPOD elif state == TURN_SECOND_TRIPOD: path = RotateFeetAboutOrigin(model, controller, [1, 3, 5], TURN_ANGLE, 2, 1) state = FEET_ON_GROUND elif state == FEET_ON_GROUND: leg_lift_height = .2 path = TrapezoidalFeetLiftLower( model, controller, range(NUM_LEGS), [i * leg_lift_height for i in LIFT_TRIPOD_024], 2, 1) state = RESOLVE elif state == RESOLVE: path = RotateFeetAboutOrigin(model, controller, range(NUM_LEGS), -TURN_ANGLE, 2, 1) state = RAISE_FIRST_TRIPOD target_angle_matrix = path.update() # controller.setDesiredJointAngles(joint_angle_matrix) # Send commands return controller.update(model.getJointAngleMatrix(), target_angle_matrix)
class RotateComposite: def __init__(self, delta_angle): self.delta_angle = delta_angle self.evens = RotateFeetAboutOrigin(model, controller, [0,2,4], delta_angle, tip_speed, tip_accel) self.odds = RotateFeetAboutOrigin(model, controller, [1,3,5], -delta_angle, tip_speed, tip_accel) def isDone(self): return(self.evens.isDone() and self.odds.isDone()) def update(self): even_commands = self.evens.update() odd_commands = self.odds.update() total_commands = [even_commands[0],odd_commands[1],even_commands[2],odd_commands[3],even_commands[4],odd_commands[5],] return(total_commands)
class RotateComposite: def __init__(self, delta_angle): delta_angle = delta_angle self.evens = RotateFeetAboutOrigin(model, controller, [0, 2, 4], delta_angle, tip_speed, tip_accel) self.odds = RotateFeetAboutOrigin(model, controller, [1, 3, 5], -delta_angle, tip_speed, tip_accel) def isDone(self): return (self.evens.isDone() and self.odds.isDone()) def update(self): even_commands = self.evens.update() odd_commands = self.odds.update() total_commands = [ even_commands[0], odd_commands[1], even_commands[2], odd_commands[3], even_commands[4], odd_commands[5], ] return (total_commands)
def __init__(self, delta_angle): delta_angle = delta_angle self.evens = RotateFeetAboutOrigin(model, controller, [0, 2, 4], delta_angle, tip_speed, tip_accel) self.odds = RotateFeetAboutOrigin(model, controller, [1, 3, 5], -delta_angle, tip_speed, tip_accel)
def __init__(self, delta_angle): self.delta_angle = delta_angle self.evens = RotateFeetAboutOrigin(model, controller, [0,2,4], delta_angle, tip_speed, tip_accel) self.odds = RotateFeetAboutOrigin(model, controller, [1,3,5], -delta_angle, tip_speed, tip_accel)