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)
def update(time, leg_sensor_matrix, imu_orientation, imu_accelerations, imu_angular_rates, command=None):
global path, state
time_sources.global_time.updateTime(time)
model.setSensorReadings(leg_sensor_matrix, imu_orientation, imu_angular_rates)
target_angle_matrix = zeros((NUM_LEGS, LEG_DOF))
# THIS IS WHERE WE CALL ON THE PATH TO DO MATH AND PRODUCE joint_angle_matrix (6x3 matrix)
if path is None:
path = BodyPause(model, controller, 1)
state = ORIENT
if path.isDone():
if state == ORIENT:
path = GoToStandardHexagon(model, controller)
state = STAND
elif state == STAND:
path = TrapezoidalSitStand(model, controller, -1.6764, 1, .1)
state = SIT
elif state == SIT:
path = TrapezoidalSitStand(model, controller, -0.6096, 1, .1)
state = STAND
elif state == 0:
path = BodyPause(model, controller, 10)
logger.info("State changed.", state=state)
# Evaluate path and joint control
target_angle_matrix = path.update()
# Send commands
return controller.update(model.getJointAngleMatrix(), target_angle_matrix)
def update(time,
leg_sensor_matrix,
imu_orientation,
imu_accelerations,
imu_angular_rates,
command=None):
global path, state
time_sources.global_time.updateTime(time)
model.setSensorReadings(leg_sensor_matrix, imu_orientation,
imu_angular_rates)
target_angle_matrix = zeros((NUM_LEGS, LEG_DOF))
# THIS IS WHERE WE CALL ON THE PATH TO DO MATH AND PRODUCE joint_angle_matrix (6x3 matrix)
if path is None:
path = BodyPause(model, controller, 1)
state = ORIENT
if path.isDone():
if state == ORIENT:
path = GoToStandardHexagon(model, controller)
state = STAND
elif state == STAND:
path = TrapezoidalSitStand(model, controller, -1.6764, 1, .1)
state = SIT
elif state == SIT:
path = TrapezoidalSitStand(model, controller, -0.6096, 1, .1)
state = STAND
elif state == 0:
path = BodyPause(model, controller, 10)
logger.info("State changed.", state=state)
# Evaluate path and joint control
target_angle_matrix = path.update()
# Send commands
return controller.update(model.getJointAngleMatrix(), target_angle_matrix)
def update(time, leg_sensor_matrix, imu_orientation, imu_accelerations, imu_angular_rates, command=None):
global path, state
time_sources.global_time.updateTime(time)
model.setSensorReadings(leg_sensor_matrix, imu_orientation, imu_angular_rates)
if path is None:
path = BodyPause(model, controller, .1)
state = ORIENT
if path.isDone():
if state == ORIENT:
path = GoToStandardHexagon(model, controller)
state = WALK
elif state == WALK:
path = TripodGait(model)
state = DONE
logger.info("State changed.", state=state)
return controller.update(model.getJointAngleMatrix(), path.update())
def update(time,
leg_sensor_matrix,
imu_orientation,
imu_accelerations,
imu_angular_rates,
command=None):
global path, state
time_sources.global_time.updateTime(time)
model.setSensorReadings(leg_sensor_matrix, imu_orientation,
imu_angular_rates)
if path is None:
path = BodyPause(model, controller, .1)
state = ORIENT
if path.isDone():
if state == ORIENT: # Put the feet in some reasonable location that they might have started from
path = GoToStandardHexagon(model, controller)
state = HALF_RAISE
elif state == HALF_RAISE: # Raise one tripod half the total travel
path = LiftComposite(-lift_height / 2)
state = HALF_TURN
elif state == HALF_TURN: # Rotate half the total rotation
path = RotateComposite(-delta_angle / 2)
state = RAISE_EVEN_TRIPOD
elif state == RAISE_EVEN_TRIPOD: # Raise a tripod
path = LiftComposite(lift_height)
state = CLOCKWISE
elif state == CLOCKWISE: # Rotate one direction
path = RotateComposite(delta_angle)
state = RAISE_ODD_TRIPOD
elif state == RAISE_ODD_TRIPOD: # Raise the other tripod
path = LiftComposite(-lift_height)
state = CCLOCKWISE
elif state == CCLOCKWISE: # Rotate the other direction
path = RotateComposite(-delta_angle)
state = RAISE_EVEN_TRIPOD
elif state == PAUSE: # Set state to PAUSE if you need to debug something
path = BodyPause(model, controller, 10)
logger.info("State changed.", state=state)
# Evaluate path and joint control
target_angle_matrix = path.update()
# Send commands
return controller.update(model.getJointAngleMatrix(), target_angle_matrix)
def update(time, leg_sensor_matrix, imu_orientation, imu_accelerations, imu_angular_rates, command=None):
global path, state
time_sources.global_time.updateTime(time)
model.setSensorReadings(leg_sensor_matrix, imu_orientation, imu_angular_rates)
if path is None:
path = BodyPause(model, controller, .1)
state = ORIENT
if path.isDone():
if state == ORIENT: # Put the feet in some reasonable location that they might have started from
path = GoToStandardHexagon(model, controller)
state = HALF_RAISE
elif state == HALF_RAISE: # Raise one tripod half the total travel
path = LiftComposite(-lift_height/2)
state = HALF_TURN
elif state == HALF_TURN: # Rotate half the total rotation
path = RotateComposite(-delta_angle/2)
state = RAISE_EVEN_TRIPOD
elif state == RAISE_EVEN_TRIPOD: # Raise a tripod
path = LiftComposite(lift_height)
state = CLOCKWISE
elif state == CLOCKWISE: # Rotate one direction
path = RotateComposite(delta_angle)
state = RAISE_ODD_TRIPOD
elif state == RAISE_ODD_TRIPOD: # Raise the other tripod
path = LiftComposite(-lift_height)
state = CCLOCKWISE
elif state == CCLOCKWISE: # Rotate the other direction
path = RotateComposite(-delta_angle)
state = RAISE_EVEN_TRIPOD
elif state == PAUSE: # Set state to PAUSE if you need to debug something
path = BodyPause(model, controller, 10)
logger.info("State changed.", state=state)
# Evaluate path and joint control
target_angle_matrix = path.update()
# Send commands
return controller.update(model.getJointAngleMatrix(), target_angle_matrix)
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)
