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)
