ewandor/kttd
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Rescue mission almost done docking part behaves weirdly)

Browse Source
This commit is contained in:
ewandor
2023-09-05 20:30:18 +02:00
parent 1499e737c1
commit 84b9d10c85
11 changed files with 377 additions and 231 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
maneuvers/approach.py
View File

@@ -1,5 +1,3 @@
from krpc.services.spacecenter import SASMode
import numpy as np
from time import time, sleep

21
maneuvers/approach_rcs.py
View File

@@ -1,9 +1,8 @@
from krpc.services.spacecenter import SASMode
import numpy as np
from time import time, sleep
from .utils import magnitude, unitary, kill_relative_velocity, kill_rcs_velocity, correct_course, get_safety_radius,\
from .utils import magnitude, unitary, kill_relative_velocity_rcs, correct_course, get_safety_radius,\
point_toward_direction
from . import Maneuver
@@ -22,7 +21,7 @@ class ApproachRCSManeuver(Maneuver):
vessel = sc.active_vessel
target = sc.target_vessel
kill_rcs_velocity(vessel, self.reference_frame)
kill_relative_velocity_rcs(vessel, target)
self.conn.drawing.add_direction((0, 1, 0), self.reference_frame)
@@ -57,9 +56,8 @@ class ApproachRCSManeuver(Maneuver):
def move_to_waypoint(conn, vessel, waypoint, reference_frame):
mj = conn.mech_jeb
kill_relative_velocity(conn, vessel, reference_frame)
target = conn.space_center.target_vessel
kill_relative_velocity_rcs(vessel, target)
conn.drawing.add_line(vessel.position(reference_frame), waypoint, reference_frame)
waypoint = np.array(waypoint)
@@ -82,14 +80,21 @@ def move_to_waypoint(conn, vessel, waypoint, reference_frame):
vessel.control.forward = 0
print("Target velocity achieved")
remaining_distance = distance - magnitude(start_position - vessel.position(reference_frame))
while remaining_distance > acceleration_distance:
sleep(.1)
correct_course(conn, vessel, waypoint, reference_frame)
remaining_distance = magnitude(waypoint - vessel.position(reference_frame))
remaining_distance = distance - magnitude(start_position - vessel.position(reference_frame))
print(remaining_distance)
print("Starting deceleration")
kill_rcs_velocity(vessel, reference_frame)
remaining_distance = distance - magnitude(start_position - vessel.position(reference_frame))
vessel.control.forward = -1
while remaining_distance > 0:
sleep(.1)
remaining_distance = distance - magnitude(start_position - vessel.position(reference_frame))
vessel.control.forward = 0
kill_relative_velocity_rcs(vessel, target)
print("Ship decelerated")
print("destination position: {}".format(waypoint))

4
maneuvers/comsat.py
View File

@@ -30,9 +30,9 @@ class ComsatManeuver(MechJebManeuver):
raise NotImplementedError
if not math.isclose(vessel.orbit.apoapsis_altitude, self.target_altitude, rel_tol=.01):
SetOrbitApoapsis(self.conn, vessel, self.target_body).prepare_maneuver()
SetOrbitApoapsis(self.conn, self.mission_control, self.target_body).prepare_maneuver()
elif not math.isclose(vessel.orbit.eccentricity, 0, abs_tol=.001) or self.vessel.control.current_stage > 1:
CircularizeOrbitAndDeliver(self.conn, vessel, self.target_body).prepare_maneuver()
CircularizeOrbitAndDeliver(self.conn, self.mission_control, self.target_body).prepare_maneuver()
elif self.vessel.control.current_stage <= 1:
return True

132
maneuvers/docking.py
View File

@@ -1,128 +1,36 @@
from time import sleep
from .utils import kill_relative_velocity, kill_rcs_velocity, correct_course, magnitude
from .utils import kill_relative_velocity, kill_relative_velocity_rcs, correct_course, magnitude
from . import Maneuver
class DockingManeuver(Maneuver):
def __init__(self, conn, mission_control, docking_part, reference_frame):
def __init__(self, conn, mission_control, docking_port, target_docking_port):
super().__init__(conn, mission_control)
self.docking_part = docking_part
self.reference_frame = reference_frame
self.mech_jeb = conn.mech_jeb
self.docking_port = docking_port
self.target_docking_port = target_docking_port
def start(self):
vessel = self.conn.space_center.active_vessel
self.conn.drawing.add_direction((0, 1, 0), self.reference_frame)
self.conn.drawing.add_direction((1, 0, 0), self.reference_frame)
vessel.parts.controlling = self.docking_part
kill_relative_velocity(self.conn, vessel, self.reference_frame)
set_attitude_and_roll(self.conn, vessel, self.reference_frame)
align_horizontally(self.conn, vessel, self.reference_frame)
self.conn.space_center.active_vessel = self.mission_control.vessel
vessel.parts.controlling = self.docking_port.part
self.conn.space_center.target_docking_port = self.target_docking_port
print("Starting docking procedure")
vessel.control.set_action_group(0, True)
rcs_push(vessel, {"y": 1}, .5)
da = self.mech_jeb.docking_autopilot
da.speed_limit = 10
da.roll = 0
da.force_roll = True
da.enabled = True
vessel.control.rcs = True
try:
while vessel.position(self.reference_frame)[1] > 0:
print(vessel.position(self.reference_frame)[1])
correct_course(self.conn, vessel, (0, 0, 0), self.reference_frame)
sleep(1)
except ValueError as e:
vessel = self.conn.space_center.active_vessel
finally:
vessel.control.rcs = False
sleep(1)
with self.conn.stream(getattr, da, "enabled") as enabled:
enabled.rate = 1
with enabled.condition:
while enabled():
enabled.wait()
return True
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 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!")

36
maneuvers/grapple.py Normal file
View File

@@ -0,0 +1,36 @@
from time import sleep
from . import Maneuver
from .utils import magnitude, correct_course_to_target, rcs_push, point_toward_target
class GrappleManeuver(Maneuver):
def __init__(self, conn, mission_control, target):
super().__init__(conn, mission_control)
self.target = target
def start(self):
vessel = self.mission_control.vessel
vessel.parts.controlling = self.mission_control.get_grappling()
point_toward_target(self.conn, vessel, self.target)
self.mission_control.toggle_grappling(True)
rcs_push(vessel, {"y": 1}, .75)
vessel.control.rcs = True
try:
while magnitude(vessel.position(self.target.reference_frame)) > 0:
print(magnitude(vessel.position(self.target.reference_frame)))
sa = self.conn.mech_jeb.smart_ass
sa.autopilot_mode = self.conn.mech_jeb.SmartASSAutopilotMode.relative_plus
sa.update(False)
correct_course_to_target(vessel, self.target)
except ValueError:
vessel = self.conn.space_center.active_vessel
self.mission_control.vessel = vessel
finally:
vessel.control.rcs = False
return True

8
maneuvers/rendezvous.py
View File

@@ -18,18 +18,18 @@ class RendezvousManeuver(MechJebManeuver):
if vessel.orbit.distance_at_closest_approach(target.orbit) > 1000:
if vessel.orbit.relative_inclination(target.orbit) > 0.0001:
AlignOrbitPlaneWithTarget(self.conn, vessel).prepare_maneuver()
AlignOrbitPlaneWithTarget(self.conn, self.mission_control).prepare_maneuver()
elif vessel.orbit.distance_at_closest_approach(target.orbit) > 10000:
InterceptTargetOrbit(self.conn, vessel).prepare_maneuver()
InterceptTargetOrbit(self.conn, self.mission_control).prepare_maneuver()
else:
TuneClosestApproach(self.conn, vessel).prepare_maneuver()
TuneClosestApproach(self.conn, self.mission_control).prepare_maneuver()
return False
elif vessel.orbit.distance_at_closest_approach(target.orbit) <= 1000 < magnitude(
np.array(vessel.position(vessel.reference_frame)) - np.array(target.position(vessel.reference_frame))):
MatchVelocityWithTarget(self.conn, vessel).prepare_maneuver()
MatchVelocityWithTarget(self.conn, self.mission_control).prepare_maneuver()
return False
else:
return True

206
maneuvers/utils.py
View File

@@ -1,7 +1,13 @@
from time import time, sleep
import math
import numpy as np
from krpc.services.spacecenter import SASMode
from connector import get_connexion
def magnitude(vector):
return np.linalg.norm(vector)
@@ -65,43 +71,92 @@ def correct_course(conn, vessel, waypoint, reference_frame):
vessel.control.up = 0
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
def correct_course_to_target(vessel, target):
target_position = target.position(vessel.reference_frame)
if abs(velo[1]) > .05:
sign = -velo[1] / abs(velo[1])
if abs(velo[1]) > .1:
angle_x = math.atan(target_position[0]/target_position[1])
if math.isclose(angle_x, 0, abs_tol=0.05):
vessel.control.right = 0
elif angle_x < 0:
vessel.control.right = -.1
elif angle_x > 0:
vessel.control.right = .1
angle_z = math.atan(target_position[2]/target_position[1])
if math.isclose(angle_z, 0, abs_tol=0.05):
vessel.control.up = 0
elif angle_z < 0:
vessel.control.up = .1
elif angle_z > 0:
vessel.control.up = -.1
def kill_relative_velocity_rcs(vessel, target):
print("Killing RCS velocity")
vessel.control.sas = True
vessel.control.rcs = True
velocity = target.velocity(vessel.reference_frame)
while any(abs(component) >= .1 for component in velocity):
thrust = get_required_rcs_thrust(vessel, velocity)
vessel.control.right = thrust[0] if abs(velocity[0]) >= .1 else 0
vessel.control.forward = thrust[1] if abs(velocity[1]) >= .1 else 0
vessel.control.up = - thrust[2] if abs(velocity[2]) >= .1 else 0
print(target.velocity(vessel.reference_frame))
print((thrust[0], thrust[1], thrust[2]))
print((vessel.control.right, vessel.control.forward, vessel.control.up))
sleep(.1)
velocity = target.velocity(vessel.reference_frame)
continue
if abs(velocity[0]) > .05:
sign = velocity[0] / abs(velocity[0])
if abs(velocity[0]) > 1:
vessel.control.right = 1 * sign
elif abs(velocity[0]) > .1:
vessel.control.right = .1 * sign
else:
vessel.control.right = 0
if abs(velocity[1]) > .05:
sign = velocity[1] / abs(velocity[1])
if abs(velocity[1]) > 1:
vessel.control.forward = 1 * sign
elif abs(velo[1]) > .05:
elif abs(velocity[1]) > .1:
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
if abs(velocity[2]) > .05:
sign = - velocity[2] / abs(velocity[2])
if abs(velocity[2]) > 1:
vessel.control.up = 1 * sign
elif abs(velocity[2]) > .1:
vessel.control.up = .1 * sign
else:
vessel.control.right = 0
vessel.control.up = 0
sleep(.1)
velo = vessel.velocity(reference_frame)
velocity = target.velocity(vessel.reference_frame)
vessel.control.rcs = False
vessel.control.sas = False
print("RCS velocity killed")
def get_required_rcs_thrust(vessel, delta_v, polling=.1):
acceleration = np.array(vessel.available_rcs_force) / vessel.mass
thrust = [0, 0, 0]
for i in range(3):
if delta_v[i] >= 0:
thrust[i] = max(min(delta_v[i] / acceleration[0][i]*polling, 1), .051)
else:
thrust[i] = min(max(-delta_v[i] / acceleration[1][i]*polling, -1), -.051)
return thrust
def get_safety_radius(vessel):
bbox = vessel.bounding_box(vessel.reference_frame)
return max(magnitude(bbox[0]), magnitude(bbox[1]))
@@ -112,6 +167,7 @@ def point_toward_direction(vessel, direction, reference_frame):
ap.reference_frame = reference_frame
ap.target_direction = unitary(direction)
ap.target_roll = 0
ap.rcs = False
ap.sas = False
ap.engage()
sleep(1)
@@ -119,4 +175,102 @@ def point_toward_direction(vessel, direction, reference_frame):
ap.disengage()
ap.sas_mode = SASMode.stability_assist
ap.sas = True
ap.sas = True
def point_toward_target(conn, vessel, target, force_roll=False):
sa = conn.mech_jeb.smart_ass
sa.autopilot_mode = conn.mech_jeb.SmartASSAutopilotMode.target_plus
sa.force_roll = force_roll
sa.update(False)
while magnitude(vessel.angular_velocity(target.reference_frame)) > .002:
sleep(.1)
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 set_attitude_and_roll(conn, vessel, 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 align_horizontally(conn, vessel, reference_frame):
conn.drawing.add_direction((1, 0, 0), vessel.reference_frame)
target = conn.space_center.target_vessel
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_relative_velocity_rcs(vessel, target)
print("Vertical alignment done!")