Example #1
0
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)
Example #2
0
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)
Example #3
0
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)
Example #4
0
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())
Example #5
0
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)
Example #6
0
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)
Example #7
0
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)