Пример #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)
Пример #2
0
class LiftComposite:
    def __init__(self, height):
        self.height = height
        self.evens = TrapezoidalFeetLiftLower(model, controller, [0,2,4], height, tip_speed, tip_accel)        
        self.odds = TrapezoidalFeetLiftLower(model, controller, [1,3,5], -height, tip_speed, tip_accel)
    def isDone(self):
        return(self.evens.isDone() and self.odds.isDone())
    def update(self):
        if (self.height < 0 and self.evens.isDone()) or (self.height > 0 and not self.odds.isDone()):
            return(self.odds.update())
        else:
            return(self.evens.update())
Пример #3
0
class LiftComposite:
    def __init__(self, height):
        self.height = height
        self.evens = TrapezoidalFeetLiftLower(model, controller, [0, 2, 4],
                                              height, tip_speed, tip_accel)
        self.odds = TrapezoidalFeetLiftLower(model, controller, [1, 3, 5],
                                             -height, tip_speed, tip_accel)

    def isDone(self):
        return (self.evens.isDone() and self.odds.isDone())

    def update(self):
        if (self.height < 0
                and self.evens.isDone()) or (self.height > 0
                                             and not self.odds.isDone()):
            return (self.odds.update())
        else:
            return (self.evens.update())
Пример #4
0
 def __init__(self, height):
     self.height = height
     self.evens = TrapezoidalFeetLiftLower(model, controller, [0, 2, 4],
                                           height, tip_speed, tip_accel)
     self.odds = TrapezoidalFeetLiftLower(model, controller, [1, 3, 5],
                                          -height, tip_speed, tip_accel)
Пример #5
0
 def __init__(self, height):
     self.height = height
     self.evens = TrapezoidalFeetLiftLower(model, controller, [0,2,4], height, tip_speed, tip_accel)        
     self.odds = TrapezoidalFeetLiftLower(model, controller, [1,3,5], -height, tip_speed, tip_accel)