From ac46f6e9e2b84f1064177da67ac52bcc66ff0c8c Mon Sep 17 00:00:00 2001 From: ewandor Date: Fri, 25 Aug 2023 00:44:19 +0200 Subject: [PATCH] Initial commit --- .idea/.gitignore | 8 + .idea/.name | 1 + .../inspectionProfiles/profiles_settings.xml | 6 + .idea/kttd.iml | 10 + .idea/misc.xml | 4 + .idea/modules.xml | 8 + calendar.py | 35 ++ console.py | 15 + main.py | 16 + maneuvers/__init__.py | 0 maneuvers/approach.py | 207 ++++++++++ maneuvers/docking.py | 156 ++++++++ maneuvers/rendezvous.py | 103 +++++ maneuvers/ssto.py | 353 ++++++++++++++++++ maneuvers/tugship.py | 65 ++++ maneuvers/utils.py | 68 ++++ 16 files changed, 1055 insertions(+) create mode 100644 .idea/.gitignore create mode 100644 .idea/.name create mode 100644 .idea/inspectionProfiles/profiles_settings.xml create mode 100644 .idea/kttd.iml create mode 100644 .idea/misc.xml create mode 100644 .idea/modules.xml create mode 100644 calendar.py create mode 100644 console.py create mode 100644 main.py create mode 100644 maneuvers/__init__.py create mode 100644 maneuvers/approach.py create mode 100644 maneuvers/docking.py create mode 100644 maneuvers/rendezvous.py create mode 100644 maneuvers/ssto.py create mode 100644 maneuvers/tugship.py create mode 100644 maneuvers/utils.py diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..13566b8 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Editor-based HTTP Client requests +/httpRequests/ +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml diff --git a/.idea/.name b/.idea/.name new file mode 100644 index 0000000..11a5d8e --- /dev/null +++ b/.idea/.name @@ -0,0 +1 @@ +main.py \ No newline at end of file diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/.idea/kttd.iml b/.idea/kttd.iml new file mode 100644 index 0000000..74d515a --- /dev/null +++ b/.idea/kttd.iml @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..ce5e6d7 --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000..c887784 --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/calendar.py b/calendar.py new file mode 100644 index 0000000..0180ceb --- /dev/null +++ b/calendar.py @@ -0,0 +1,35 @@ +class Timeslot: + def __init__(self, ut_start, duration): + self.ut_start = ut_start + self.duration = duration + + @property + def ut_end(self): + return self.ut_start + self.duration + + @ut_end.setter + def ut_end(self, value): + self.duration = self.value - self.start + + +class Calendar: + def create_reservation(self, ut_start, duration, maneuver): + if not self.timeslot_is_free(ut_start, duration): + raise + pass + + def timeslot_is_free(self, ut_start: int, duration: int) -> bool: + pass + + def next_free_timeslot(self, from_ut, duration=None) -> int: + pass + + def get_reservation(self, ut_at) -> Timeslot: + pass + + def delete_reservation(self, ut_at, priority): + reservation = self.get_re(ut_at) + if priority <= reservation.priority: + raise + + diff --git a/console.py b/console.py new file mode 100644 index 0000000..1217b94 --- /dev/null +++ b/console.py @@ -0,0 +1,15 @@ +from time import sleep +import numpy as np +from simple_pid import PID +import krpc +conn = krpc.connect() +sc = conn.space_center +vessel = sc.active_vessel +docking_part = vessel.parts.root.children[0].children[10].children[0].children[0].children[0].children[0] +target = sc.target_vessel + +mj = conn.mech_jeb +sa = mj.smart_ass +reference_frame = sc.ReferenceFrame.create_relative(target.reference_frame, rotation=(1., 0., 0., 0.)) +conn.drawing.add_direction((0, 1, 0), reference_frame) + diff --git a/main.py b/main.py new file mode 100644 index 0000000..20a033a --- /dev/null +++ b/main.py @@ -0,0 +1,16 @@ +# This is a sample Python script. + +# Press Maj+F10 to execute it or replace it with your code. +# Press Double Shift to search everywhere for classes, files, tool windows, actions, and settings. + + +def print_hi(name): + # Use a breakpoint in the code line below to debug your script. + print(f'Hi, {name}') # Press Ctrl+F8 to toggle the breakpoint. + + +# Press the green button in the gutter to run the script. +if __name__ == '__main__': + print_hi('PyCharm') + +# See PyCharm help at https://www.jetbrains.com/help/pycharm/ diff --git a/maneuvers/__init__.py b/maneuvers/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/maneuvers/approach.py b/maneuvers/approach.py new file mode 100644 index 0000000..c3b4b42 --- /dev/null +++ b/maneuvers/approach.py @@ -0,0 +1,207 @@ +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)))) \ No newline at end of file diff --git a/maneuvers/docking.py b/maneuvers/docking.py new file mode 100644 index 0000000..5dc42dc --- /dev/null +++ b/maneuvers/docking.py @@ -0,0 +1,156 @@ +from time import sleep + +from utils import kill_relative_velocity, correct_course, magnitude + + +def set_attitude_and_roll(conn, vessel, reference_frame): + fl = vessel.flight(reference_frame) + vessel.control.rcs = False + + ap = vessel.auto_pilot + ap.reference_frame = reference_frame + ap.target_direction = (0, -1, 0) + ap.target_roll = 0 + ap.sas = False + ap.engage() + ap.wait() + ap.disengage() + + mj = conn.mech_jeb + sa = mj.smart_ass + + sa.autopilot_mode = mj.SmartASSAutopilotMode.target_plus + sa.update(False) + + while magnitude(vessel.angular_velocity(reference_frame)) > .1: + sleep(.1) + + print("Ship pointing to dock") + + +def rcs_push(vessel, axis, duration): + vessel.control.rcs = True + if "x" in axis: + vessel.control.up = axis["x"] + elif "y" in axis: + vessel.control.forward = axis["y"] + elif "z" in axis: + vessel.control.right = axis["z"] + sleep(duration) + if "x" in axis: + vessel.control.up = 0 + elif "y" in axis: + vessel.control.forward = 0 + elif "z" in axis: + vessel.control.right = 0 + + vessel.control.rcs = False + + +def kill_rcs_velocity(vessel, reference_frame): + print("Killing RCS velocity") + velo = vessel.velocity(reference_frame) + vessel.control.rcs = True + while any(abs(component) > .05 for component in velo) > .05: + if abs(velo[0]) > .05: + sign = -velo[0] / abs(velo[0]) + if abs(velo[0]) > .1: + vessel.control.up = 1 * sign + elif abs(velo[0]) > .05: + vessel.control.up = .1 * sign + else: + vessel.control.up = 0 + + if abs(velo[1]) > .05: + sign = -velo[1] / abs(velo[1]) + if abs(velo[1]) > .1: + vessel.control.forward = 1 * sign + elif abs(velo[1]) > .05: + vessel.control.forward = .1 * sign + else: + vessel.control.forward = 0 + + if abs(velo[2]) > .05: + sign = velo[2] / abs(velo[2]) + if abs(velo[2]) > .1: + vessel.control.right = 1 * sign + elif abs(velo[2]) > .05: + vessel.control.right = .1 * sign + else: + vessel.control.right = 0 + sleep(.1) + velo = vessel.velocity(reference_frame) + vessel.control.rcs = False + print("RCS velocity killed") + + +def align_horizontally(conn, vessel, reference_frame): + conn.drawing.add_direction((1, 0, 0), vessel.reference_frame) + + while abs(vessel.position(reference_frame)[0]) > .1 \ + or abs(vessel.position(reference_frame)[2]) > .1: + # determine power requirements of each + sign_x = 1 if vessel.position(reference_frame)[0] > 0 else -1 + if abs(vessel.position(reference_frame)[0]) > 1: + power_x = 1 + elif abs(vessel.position(reference_frame)[0]) > .1: + power_x = .1 + else: + power_x = 0 + sign_x = 0 + + sign_z = 1 if vessel.position(reference_frame)[2] > 0 else -1 + if abs(vessel.position(reference_frame)[2]) > 1: + power_z = 1 + elif abs(vessel.position(reference_frame)[2]) > .1: + power_z = .1 + else: + power_z = 0 + sign_z = 0 + + axis = {} + if power_x > 0: + axis["x"] = -sign_x * power_x + if power_z > 0: + axis["z"] = sign_z * power_z + + rcs_push(vessel, axis, 1) + + while (sign_x > 0 and vessel.position(reference_frame)[0] > .1 + or sign_x < 0 and vessel.position(reference_frame)[0] < -.1 + or sign_x == 0) \ + and (sign_z > 0 and vessel.position(reference_frame)[2] > .1 + or sign_z < 0 and vessel.position(reference_frame)[2] < -.1 + or sign_z == 0): + print(vessel.position(reference_frame)) + sleep(.1) + + kill_rcs_velocity(vessel, reference_frame) + print("Vertical alignment done!") + + +def dock_ship(conn, vessel, docking_part, reference_frame): + conn.drawing.add_direction((0, 1, 0), reference_frame) + conn.drawing.add_direction((1, 0, 0), reference_frame) + vessel.parts.controlling = docking_part + + kill_relative_velocity(conn, vessel, reference_frame) + + set_attitude_and_roll(conn, vessel, reference_frame) + + align_horizontally(conn, vessel, reference_frame) + + print("Starting docking procedure") + vessel.control.set_action_group(0, True) + rcs_push(vessel, {"y": 1}, .5) + + vessel.control.rcs = True + try: + while vessel.position(reference_frame)[1] > 0: + print(vessel.position(reference_frame)[1]) + correct_course(conn, vessel, (0, 0, 0), reference_frame) + sleep(1) + except ValueError as e: + vessel = conn.space_center.active_vessel + finally: + vessel.control.rcs = False diff --git a/maneuvers/rendezvous.py b/maneuvers/rendezvous.py new file mode 100644 index 0000000..8d77465 --- /dev/null +++ b/maneuvers/rendezvous.py @@ -0,0 +1,103 @@ +import numpy as np + +from utils import execute_node, magnitude + + +def align_orbit_planes(conn): + print("Aligning planes") + mj = conn.mech_jeb + mp = mj.maneuver_planner + oi = mp.operation_inclination + oi.time_selector.time_reference = mj.TimeReference.eq_nearest_ad + nodes = oi.make_nodes() + + # kac = conn.kerbal_alarm_clock + # kac.create_alarm( + # kac.AlarmType.maneuver, + # "{}'s Orbital transfer".format(v.name), + # nodes[0].ut + # ) + + execute_node(conn) + + print("Planes aligned!") + + +def intercepting_target_orbit(conn): + print("Intercepting target orbit") + sc = conn.space_center + v = sc.active_vessel + mj = conn.mech_jeb + mp = mj.maneuver_planner + ot = mp.operation_transfer + ot.time_selector.time_reference = mj.TimeReference.computed + nodes = ot.make_nodes() + nodes[0].ut = nodes[0].ut + 0.1 + + # kac = conn.kerbal_alarm_clock + # kac.create_alarm( + # kac.AlarmType.maneuver, + # "{}'s Orbital transfer".format(v.name), + # nodes[0].ut + # ) + + execute_node(conn) + + print("Target orbit intercepted!") + + +def tune_closest_approach(conn): + print("Tuning closest approach") + sc = conn.space_center + v = sc.active_vessel + mj = conn.mech_jeb + mp = mj.maneuver_planner + occ = mp.operation_course_correction + nodes = occ.make_nodes() + + # kac = conn.kerbal_alarm_clock + # kac.create_alarm( + # kac.AlarmType.maneuver, + # "{}'s Orbital transfer".format(v.name), + # nodes[0].ut + # ) + + execute_node(conn) + + print("Closest approach tuned!") + + +def match_velocities(conn): + print("Matching velocities") + sc = conn.space_center + v = sc.active_vessel + mj = conn.mech_jeb + mp = mj.maneuver_planner + okrv = mp.operation_kill_rel_vel + nodes = okrv.make_nodes() + + # kac = conn.kerbal_alarm_clock + # kac.create_alarm( + # kac.AlarmType.maneuver, + # "{}'s Orbital transfer".format(v.name), + # nodes[0].ut + # ) + + execute_node(conn) + + print("Velocities matched!") + + +def maneuver_to_rendezvous(conn, vessel, target): + if vessel.orbit.distance_at_closest_approach(target.orbit) > 1000: + if vessel.orbit.relative_inclination(target.orbit) > 0.0001: + align_orbit_planes(conn) + + if vessel.orbit.distance_at_closest_approach(target.orbit) > 10000: + intercepting_target_orbit(conn) + + tune_closest_approach(conn) + + if vessel.orbit.distance_at_closest_approach(target.orbit) <= 1000 < magnitude( + np.array(vessel.position(vessel.reference_frame)) - np.array(target.position(vessel.reference_frame))): + match_velocities(conn) diff --git a/maneuvers/ssto.py b/maneuvers/ssto.py new file mode 100644 index 0000000..58b025d --- /dev/null +++ b/maneuvers/ssto.py @@ -0,0 +1,353 @@ +from time import sleep + +import numpy as np +import numpy.linalg as la + +from simple_pid import PID +import krpc + + +conn = krpc.connect() +print(conn.krpc.get_status().version) + +START_ALIGNMENT_SPEED = 900 + + +def get_surface_prograde_pitch(sc, vessel): + import math + + def cross(u, v): + return ( + u[1] * v[2] - u[2] * v[1], + u[2] * v[0] - u[0] * v[2], + u[0] * v[1] - u[1] * v[0]) + + def dot(u, v): + return u[0] * v[0] + u[1] * v[1] + u[2] * v[2] + + # these vectors are all in vessel.surface_reference_frame + north = (0, 1, 0) + east = (0, 0, 1) + + surface_prograde = sc.transform_direction( + (0, 1, 0), + vessel.surface_velocity_reference_frame, + vessel.surface_reference_frame) + + plane_normal = cross(north, east) + + return math.asin(dot(plane_normal, surface_prograde)) * (180.0 / math.pi) + + +def lift_off(conn, altitude=100000): + sc = conn.space_center + mj = conn.mech_jeb + ascent = mj.ascent_autopilot + + ascent.desired_orbit_altitude = altitude + ascent.desired_inclination = 0 + + ascent.force_roll = True + ascent.vertical_roll = 0 + ascent.turn_roll = 0 + + ascent.autostage = False + ascent.skip_circularization = True + ascent.enabled = True + sc.active_vessel.control.activate_next_stage() + + print("We have lift off!") + + +def orbit(conn): + sc = conn.space_center + mj = conn.mech_jeb + ascent = mj.ascent_autopilot + planner = mj.maneuver_planner + + v = sc.active_vessel + + with conn.stream(getattr, ascent, "status") as status: + status.rate = 1 + with status.condition: + while status() != 'Off': + print(status()) + status.wait() + + v.control.rcs = True + v.control.set_action_group(1, True) + + executor = mj.node_executor + executor.tolerance = 1 + + circ = planner.operation_circularize + circ.time_selector.time_reference = mj.TimeReference.apoapsis + circ.make_nodes() + executor.execute_all_nodes() + + with conn.stream(getattr, executor, "enabled") as enabled: + enabled.rate = 1 + with enabled.condition: + while enabled(): + enabled.wait() + + print("Orbit complete!") + + +def dropping_payload(conn): + print("Dropping payload") + sc = conn.space_center + v = sc.active_vessel + ctrl = v.control + + payload = ctrl.activate_next_stage() + payload = payload[0] + + ctrl.up = -1 + with conn.stream(payload.position, v.reference_frame) as payload_position: + payload_position.rate = 1 + far_enough = False + while not far_enough: + displacement = np.array(payload_position()) + distance = la.norm(displacement) + far_enough = distance > 20 + + ctrl.up = 0 + print("Payload Dropped") + + +def is_longitude_in_target_range(long, target, range): + if target - range <= -180: + return long < target + range or long > 180 - (-180 - (target - range)) + elif target + range > 180: + return long > target - range or long < -180 + (180 - (target + range)) + + return target - range < long < target + range + + +TARGET_DEORBIT_NODE_LONGITUDE = 178 + + +def deorbit(conn): + sc = conn.space_center + v = sc.active_vessel + o = v.orbit + b = o.body + + surface_rf = b.reference_frame + + node = v.control.add_node(sc.ut + 300, -100) + + longitude_at = b.longitude_at_position(o.position_at(node.ut, surface_rf), surface_rf) + while not is_longitude_in_target_range(longitude_at, TARGET_DEORBIT_NODE_LONGITUDE, 10): + node.ut = node.ut + 10 + longitude_at = b.longitude_at_position(o.position_at(node.ut, surface_rf), surface_rf) + while not is_longitude_in_target_range(longitude_at, TARGET_DEORBIT_NODE_LONGITUDE, 1): + node.ut = node.ut + 1 + longitude_at = b.longitude_at_position(o.position_at(node.ut, surface_rf), surface_rf) + + print("Maneuver created") + mj = conn.mech_jeb + executor = mj.node_executor + executor.tolerance = 1 + executor.execute_one_node() + + with conn.stream(getattr, executor, "enabled") as enabled: + enabled.rate = 1 + with enabled.condition: + while enabled(): + enabled.wait() + + v.control.activate_next_stage() + sc.rails_warp_factor = 7 + print("Ship deorbited") + + +REENTRY_PITCH_0_SPEED = 1900 +REENTRY_PITCH_PROP_SPEED = 600 + +CHECKPOINTS = [30000, 20000, 10000] + + +def reentry(conn): + print("Handling reentry") + sc = conn.space_center + v = sc.active_vessel + mj = conn.mech_jeb + sa = mj.smart_ass + + sa.autopilot_mode = mj.SmartASSAutopilotMode.surface + sa.surface_heading = 90 + sa.force_heading = True + sa.force_pitch = True + sa.surface_roll = 0 + sa.force_roll = True + sa.update(True) + + altitude = v.flight().surface_altitude + old_altitude = altitude + with conn.stream(v.velocity, v.orbit.body.reference_frame) as velocity: + velocity.rate = 1 + with velocity.condition: + speed = la.norm(np.array(velocity())) + while speed > START_ALIGNMENT_SPEED: + if speed < REENTRY_PITCH_0_SPEED: + if sa.surface_pitch > 0: + sa.surface_pitch = 0 + sa.update(False) + else: + if sa.surface_pitch != 5: + sa.surface_pitch = 5 + sa.update(False) + + altitude = v.flight().surface_altitude + for point in CHECKPOINTS: + if old_altitude > point > altitude: + # sc.save(str(point)) + # sc.quicksave() + print("Checkpoint {}: v={} t={}".format(point, speed, v.parts.all[11].skin_temperature)) + break + old_altitude = altitude + + + velocity.wait() + speed = la.norm(np.array(velocity())) + + v.control.rcs = False + print("Reentry completed") + + +def alignment(conn): + print("Starting alignment") + sc = conn.space_center + v = sc.active_vessel + b = v.orbit.body + mj = conn.mech_jeb + sa = mj.smart_ass + ref = b.reference_frame + + target_lat = -0.0485997 + factor = 5 + limit_heading = 5 + limit_roll = 3 + pid_h = PID(9, .005, .05, setpoint=target_lat, output_limits=[-limit_heading / factor, limit_heading / factor]) + + with open('track.csv', 'w') as track_file: + while v.flight().surface_altitude > 4000: + current_lat = b.latitude_at_position(v.position(ref), ref) + diff = str(target_lat - current_lat).replace('.', ',') + print(target_lat - current_lat) + track_file.write("{};\n".format(diff)) + control = pid_h(current_lat) + + correction = control * factor + sa.surface_heading = 90 - correction + sa.surface_roll = min(-correction, limit_roll) if correction < 0 else max(-correction, -limit_roll) + sa.surface_pitch = min(get_surface_prograde_pitch(sc, v) + 7, 0) + + sa.update(False) + sleep(1) + + v.control.gear = True + print("Alignment done") + + +def final_approach(conn): + print("Final approach") + sc = conn.space_center + v = sc.active_vessel + b = v.orbit.body + mj = conn.mech_jeb + sa = mj.smart_ass + ref = b.reference_frame + + airstrip_position = b.surface_position(-0.0485997, -74.724375, ref) + airstrip_altitude = b.surface_height(0-.0485997, -74.724375) + target_vspeed = 40 + + h_speed = v.flight(ref).horizontal_speed + + target_lat = -0.0485997 + factor = 5 + limit_heading = 5 + limit_roll = 3 + pid_h = PID(9, .005, .05, setpoint=target_lat, output_limits=[-limit_heading / factor, limit_heading / factor]) + pid_vh = PID(.5, .1, .05, setpoint=h_speed, output_limits=[0, 1]) + while v.flight().surface_altitude > 100: + ship_position = v.position(ref) + conn.drawing.add_line(airstrip_position, ship_position, ref) + distance = np.linalg.norm(np.array(ship_position) - np.array(airstrip_position)) + height = v.flight(ref).mean_altitude - airstrip_altitude + + # angle = math.acos(height/distance) + + current_vspeed = v.flight(v.orbit.body.reference_frame).vertical_speed + current_hspeed = v.flight(v.orbit.body.reference_frame).horizontal_speed + + # target_hspeed = current_vspeed * math.cos(angle) * -1 + target_hspeed = current_vspeed * (distance / height) * -1 + pid_vh.setpoint = target_hspeed + control = pid_vh(current_hspeed) + + v.control.throttle = control + + current_lat = b.latitude_at_position(v.position(ref), ref) + control = pid_h(current_lat) + + correction = control * factor + sa.surface_heading = 90 - correction + sa.surface_roll = min(-correction, limit_roll) if correction < 0 else max(-correction, -limit_roll) + sa.surface_pitch = min(get_surface_prograde_pitch(sc, v) + 7, 0) + + sa.update(False) + sleep(1) + + # pid_vs = PID(.5, .1, .05, setpoint=5) + while v.situation == sc.VesselSituation.flying: + # current_vertical_speed = v.flight(ref).vertical_speed + # control_pitch = pid_vs(current_vertical_speed) + sa.surface_heading = 90 + sa.surface_roll = 0 + sa.surface_pitch = 0 + sa.update(False) + + sleep(1) + + v.control.brakes = True + sa.autopilot_mode = mj.SmartASSAutopilotMode.off + + print("Landed!!") + + +# check status to determine course of actions +sc = conn.space_center +v = sc.active_vessel +if v.situation == sc.VesselSituation.pre_launch: + lift_off(conn) + +mj = conn.mech_jeb +ascent = mj.ascent_autopilot +if ascent.status and ascent.status != 'Off': + orbit(conn) + +if v.control.current_stage == 3: + dropping_payload(conn) + +if v.control.current_stage == 2 and v.situation == sc.VesselSituation.orbiting: + deorbit(conn) + +current_velocity = la.norm(np.array(v.velocity(v.orbit.body.reference_frame))) +is_in_reentry = v.situation == sc.VesselSituation.sub_orbital \ + or v.situation == sc.VesselSituation.flying and current_velocity > START_ALIGNMENT_SPEED + +if v.control.current_stage == 1 and is_in_reentry: + reentry(conn) + +if v.situation == sc.VesselSituation.flying and v.flight().surface_altitude > 4000: + alignment(conn) + +final_approach(conn) + +conn.close() + + diff --git a/maneuvers/tugship.py b/maneuvers/tugship.py new file mode 100644 index 0000000..d37ee77 --- /dev/null +++ b/maneuvers/tugship.py @@ -0,0 +1,65 @@ +from time import sleep + +import numpy as np +import numpy.linalg as la + +# from docking import * +from utils import magnitude +from rendezvous import maneuver_to_rendezvous +from approach import maneuver_to_approach +from docking import dock_ship +import krpc + + +conn = krpc.connect() +print(conn.krpc.get_status().version) + + +def undock(conn): + print('Undocking') + sc = conn.space_center + v = sc.active_vessel + target = sc.target_vessel + + decouplers = [d for d in v.parts.decouplers if d.staged and not d.decoupled] + + tugship = decouplers[-1].decouple() + sc.active_vessel = tugship + v = sc.active_vessel + v.name = "Tugship " + target.name + sc.target_vessel = target + + v.control.throttle = 0 + + v.control.rcs = True + v.control.up = -1 + sleep(5) + v.control.up = 0 + + v.control.solar_panels = True + v.parts.engines[0].active = True + print('Undocking sequence finished') + + +sc = conn.space_center +vessel = sc.active_vessel +target = sc.target_vessel + +if vessel.name == "TugGrape": + if target is None: + print("Must set a target") + exit(1) + + undock(conn) + +maneuver_to_rendezvous(conn, vessel, target) + +reference_frame = sc.ReferenceFrame.create_relative(target.reference_frame, rotation=(1., 0., 0., 0.)) + +if magnitude(vessel.position(vessel.reference_frame) - np.array(target.position(vessel.reference_frame))) < 1000: + maneuver_to_approach(conn, reference_frame) + +docking_part = vessel.parts.root.children[0].children[10].children[0].children[0].children[0].children[0] +dock_ship(conn, vessel, docking_part, reference_frame) + +conn.close() diff --git a/maneuvers/utils.py b/maneuvers/utils.py new file mode 100644 index 0000000..17b754c --- /dev/null +++ b/maneuvers/utils.py @@ -0,0 +1,68 @@ +from time import time, sleep + +import numpy as np + + +def execute_node(conn): + ne = conn.mech_jeb.node_executor + ne.execute_all_nodes() + + with conn.stream(getattr, ne, "enabled") as enabled: + enabled.rate = 1 + with enabled.condition: + while enabled(): + enabled.wait() + + +def magnitude(vector): + return np.linalg.norm(vector) + + +THROTTLE = .1 + + +def kill_relative_velocity(conn, vessel, reference_frame): + mj = conn.mech_jeb + sa = mj.smart_ass + + vessel.control.throttle = 0 + print("Killing relative velocity") + while magnitude(vessel.velocity(reference_frame)) > .05: + sa.autopilot_mode = mj.SmartASSAutopilotMode.relative_minus + sa.update(False) + while magnitude(vessel.angular_velocity(reference_frame)) > .1: + sleep(.1) + + vessel.control.throttle = THROTTLE if magnitude(vessel.velocity(reference_frame)) > 1 else THROTTLE / 10 + current_speed = magnitude(vessel.velocity(reference_frame)) + previous_speed = current_speed + while current_speed <= previous_speed: + sleep(.1) + previous_speed = current_speed + current_speed = magnitude(vessel.velocity(reference_frame)) + + vessel.control.throttle = 0 + + print("Relative velocity killed") + sa.autopilot_mode = mj.SmartASSAutopilotMode.off + sa.update(False) + + +def correct_course(conn, vessel, waypoint, reference_frame): + waypoint = np.array(conn.space_center.transform_position(tuple(waypoint), reference_frame, vessel.reference_frame)) + + waypoint_x = round(waypoint[0], 0) + if waypoint_x < 0: + vessel.control.right = -.1 + elif waypoint_x > 0: + vessel.control.right = .1 + else: + vessel.control.right = 0 + + waypoint_z = round(waypoint[2], 0) + if waypoint_z < 0: + vessel.control.up = .1 + elif waypoint_z > 0: + vessel.control.up = -.1 + else: + vessel.control.up = 0