kttd/maneuvers/approach.py

from krpc.services.spacecenter import SASMode

import numpy as np
from time import time, sleep

from utils import magnitude, kill_relative_velocity, correct_course


def unitary(vector):
    return np.array(vector) / magnitude(vector)


def get_safety_radius(vessel):
    bbox = vessel.bounding_box(vessel.reference_frame)
    return max(magnitude(bbox[0]), magnitude(bbox[1]))


def point_toward_direction(vessel, direction, reference_frame):
    ap = vessel.auto_pilot
    ap.reference_frame = reference_frame
    ap.target_direction = unitary(direction)
    ap.target_roll = 0
    ap.sas = False
    ap.engage()
    sleep(.1)

    while magnitude(vessel.angular_velocity(reference_frame)) > .1:
        sleep(.1)

    ap.disengage()
    ap.sas_mode = SASMode.stability_assist
    ap.sas = True


THROTTLE = .1
VELOCITY_TOLERANCE = .1


def thrust(vessel, delta_v, reference_frame):
    print("Starting velocity change")
    starting_velocity = magnitude(vessel.velocity(reference_frame))
    vessel.control.throttle = THROTTLE

    if delta_v < 0:
        while magnitude(vessel.velocity(reference_frame)) - starting_velocity > delta_v + VELOCITY_TOLERANCE:
            while magnitude(vessel.velocity(reference_frame)) - starting_velocity > delta_v + .3:
                sleep(.01)
            vessel.control.throttle = THROTTLE / 10
            sleep(.01)
    else:
        while magnitude(vessel.velocity(reference_frame)) - starting_velocity < delta_v - VELOCITY_TOLERANCE:
            while magnitude(vessel.velocity(reference_frame)) - starting_velocity < delta_v - .3:
                sleep(.01)
            vessel.control.throttle = THROTTLE / 10
            sleep(.01)

    vessel.control.throttle = 0
    print("Velocity change achieved")


def move_to_waypoint(conn, vessel, waypoint, reference_frame):
    mj = conn.mech_jeb
    sa = mj.smart_ass

    kill_relative_velocity(conn, vessel, reference_frame)

    conn.drawing.add_line(vessel.position(reference_frame), waypoint, reference_frame)
    waypoint = np.array(waypoint)

    distance = magnitude(waypoint - vessel.position(reference_frame))
    if distance > 250:
        velocity = 10
    elif distance > 50:
        velocity = 5
    elif distance > 25:
        velocity = 2
    else:
        velocity = 1

    direction = waypoint - np.array(vessel.position(reference_frame))
    point_toward_direction(vessel, direction, reference_frame)

    start_position = np.array(vessel.position(reference_frame))
    print("Starting acceleration")
    thrust(vessel, velocity, reference_frame)
    print("Target velocity achieved")
    acceleration_distance = magnitude(np.array(vessel.position(reference_frame)) - start_position)

    sa.autopilot_mode = mj.SmartASSAutopilotMode.relative_minus
    sa.update(False)

    while magnitude(vessel.angular_velocity(reference_frame)) > .1:
        sleep(.1)

    vessel.control.rcs = True
    while magnitude(waypoint - vessel.position(reference_frame)) > acceleration_distance:
        print(magnitude(waypoint - vessel.position(reference_frame)), " ", acceleration_distance)
        sleep(.1)
        correct_course(conn, vessel, waypoint, reference_frame)

    vessel.control.rcs = False
    vessel.control.up = 0
    vessel.control.right = 0

    print("Starting deceleration")
    thrust(vessel, -velocity, reference_frame)
    print("Ship decelerated")

    #do positition correction

    sa.autopilot_mode = mj.SmartASSAutopilotMode.off
    sa.update(True)

    print("destination position: {}".format(waypoint))
    print("end position:         {}".format(np.array(vessel.position(reference_frame))))


SAFETY_RADIUS_MARGIN = 10


def maneuver_to_approach(conn, reference_frame):
    print("Handling approach")
    sc = conn.space_center
    vessel = sc.active_vessel
    target = sc.target_vessel
    kill_relative_velocity(conn, vessel, reference_frame)
    conn.drawing.add_direction((0, 1, 0), reference_frame)

    vessel.control.rcs = False

    pv = vessel.position(reference_frame)

    safety_radius = get_safety_radius(vessel) + get_safety_radius(target) + SAFETY_RADIUS_MARGIN

    # if under and inside safety cylinder's circle
    if pv[1] < safety_radius and pow(pv[0], 2) + pow(pv[2], 2) <= pow(safety_radius, 2):
        print("We're under the target and inside the safety cylinder, getting out")
        # get out of the cylinder
        plane_move_vector = unitary(tuple((pv[0], pv[2]))) * (safety_radius - magnitude(tuple((pv[0], pv[2]))))

        pv = vessel.position(reference_frame)
        move_vector = np.array((plane_move_vector[0], 0, plane_move_vector[1]))
        move_to_waypoint(conn, vessel, pv + move_vector, reference_frame)

    print("We're outside of the safety cylinder, setting vertical distance")
    pv = vessel.position(reference_frame)
    move_to_waypoint(conn, vessel, (pv[0], safety_radius, pv[2]), reference_frame)

    # should be above and outside => get inside
    print("We're at the right vertical distance to the target, setting horizontal position")
    move_to_waypoint(conn, vessel, (0, safety_radius, 0), reference_frame)

    point_toward_direction(vessel, - np.array(vessel.position(reference_frame)), reference_frame)

    print("Approach handled")


TARGET_VELOCITY = 2


def move_with_vector(conn, vessel, vector, reference_frame):
    mj = conn.mech_jeb
    sa = mj.smart_ass

    kill_relative_velocity(conn, vessel, reference_frame)

    position = np.array(vessel.position(reference_frame))
    vector = np.array(vector)
    destination = position + vector

    conn.drawing.add_line(vessel.position(reference_frame), destination, reference_frame)

    print("Pointing to acceleration")
    point_toward_direction(vessel, unitary(vector), reference_frame)
    print("Pointed")
    trip_duration = magnitude(vector) / TARGET_VELOCITY
    acceleration_start = time()
    print("Starting acceleration")
    vessel.control.throttle = THROTTLE
    while magnitude(vessel.velocity(reference_frame)) < TARGET_VELOCITY:
        sleep(.01)
    vessel.control.throttle = 0
    print("Target velocity achieved")

    acceleration_duration = time() - acceleration_start

    sa.autopilot_mode = mj.SmartASSAutopilotMode.relative_minus
    sa.update(False)

    while acceleration_duration < trip_duration - (time() - acceleration_start):
        sleep(.01)

    print("Starting deceleration")
    vessel.control.throttle = THROTTLE

    deceleration_start = time()
    while time() - deceleration_start < acceleration_duration:
        sleep(.01)
    vessel.control.throttle = 0
    print("Deceleration done")

    sa.autopilot_mode = mj.SmartASSAutopilotMode.off
    sa.update(False)

    print("starting position:    {}".format(position))
    print("destination position: {}".format(destination))
    print("end position:         {}".format(np.array(vessel.position(reference_frame))))