kRPC AutoPilot

autopilot.py

import krpc
import time
import numpy
import math


def main():
    conn = krpc.connect()
    vessel = conn.space_center.active_vessel
    # Point prograde, 0 degree roll
    ap = AutoPilot(conn, vessel, vessel.orbital_reference_frame, (0,1,0), 0)
    while True:
        ap.update()


class AutoPilot(object):

    def __init__(self, conn, vessel, reference_frame, target_direction = None, target_roll = None):
        self.conn = conn
        self.vessel = vessel
        self.control = vessel.control
        self.reference_frame = reference_frame
        self.target_direction = target_direction
        self.target_roll = target_roll

        # Tunable parameters
        self.rotation_speed_multiplier = 1
        self.max_rotation_speed = 1
        self.roll_speed_multiplier = 1
        self.max_roll_speed = 1
        self.kp = 1
        self.ki = 0
        self.kd = 0

        self.controller = RotationRateController(conn, vessel, reference_frame)
        self.controller.pid.set_parameters(self.kp,self.ki,self.kd)

    def update(self):
        current_direction = self.conn.space_center.transform_direction(
            (0,1,0), self.vessel.reference_frame, self.reference_frame)

        target_rotation = numpy.array((0,0,0))

        # add rotation to get target direction
        if self.target_direction != None:
            # compute target angular velocity, with sin(theta) drop off as it approaches the target
            rotation = numpy.cross(self.target_direction, current_direction)

            # set target angular velocity to magnitude of 1 if pointing in opposite direction
            #TODO: kRPC doesn't do this yet, but probably should...
            if numpy.dot(self.target_direction, current_direction) < 0:
                rotation /= numpy.linalg.norm(rotation)

            rotation *= self.rotation_speed_multiplier
            if numpy.linalg.norm(rotation) > self.max_rotation_speed:
                rotation /= numpy.linalg.norm(rotation)
                rotation *= self.max_rotation_speed
            target_rotation = target_rotation + rotation

        # add rotation to get target roll
        if self.target_roll != None:
            current_roll = self.vessel.flight(self.reference_frame).roll
            roll_error = norm_angle(self.target_roll - current_roll) * (math.pi/180.0)
            rotation = numpy.array(self.target_direction) * (roll_error * self.roll_speed_multiplier)
            rotation = numpy.maximum(-self.max_roll_speed, numpy.minimum(self.max_roll_speed, rotation))
            target_rotation = target_rotation + rotation

        # run the angular velocity controller
        self.controller.target = target_rotation
        inputs = self.controller.update()
        self.control.pitch = float(inputs[0])
        self.control.yaw = float(inputs[1])
        self.control.roll = float(inputs[2])


class PIDController(object):
    """ Robust, single parameter, proportional-integral-derivative controller
        http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ """

    def __init__(self, Kp = 1, Ki = 0, Kd = 0, dt = 1):
        self.set_parameters(Kp, Ki, Kd, dt)
        self.Ti = 0
        self.last_position = 0

    def set_parameters(self, Kp = 1, Ki = 0, Kd = 0, dt = 1):
        self.Kp = Kp
        self.Ki = Ki * dt
        self.Kd = Kd / dt

    def update(self, error, position, min_output, max_output):
        self.Ti += self.Ki * error
        self.Ti = numpy.maximum(min_output, numpy.minimum(max_output, self.Ti))
        dInput = position - self.last_position
        output = self.Kp * error + self.Ti - self.Kd * dInput
        output = numpy.maximum(min_output, numpy.minimum(max_output, output))
        self.last_position = position
        return output


class RotationRateController(object):
    """ Uses a PID to try and match a target angular velocity """

    def __init__(self, conn, vessel, reference_frame):
        self.conn = conn
        self.vessel = vessel
        self.reference_frame = reference_frame
        self.pid = PIDController()
        self.target = None

    @property
    def error(self):
        velocity = numpy.array(self.vessel.angular_velocity(self.reference_frame))
        error = numpy.array(self.target) - velocity
        pitchAxis = self.conn.space_center.transform_direction(
            (1,0,0), self.vessel.reference_frame, self.reference_frame)
        yawAxis = self.conn.space_center.transform_direction(
            (0,0,1), self.vessel.reference_frame, self.reference_frame)
        rollAxis = self.conn.space_center.transform_direction(
            (0,1,0), self.vessel.reference_frame, self.reference_frame)
        pitch = numpy.dot(error, pitchAxis)
        yaw = numpy.dot(error, yawAxis)
        roll = numpy.dot(error, rollAxis)
        return (pitch, yaw, roll)

    def update(self):
        return tuple(self.pid.update(numpy.array(self.error), numpy.array(self.target), -1.0, 1.0))


def norm_angle(angle):
    """ Normalize angle to the range (-180,180) """
    return angle - 360.0 * math.floor ((angle + 180.0) / 360.0)


if __name__ == "__main__":
    main()