Initial commit
This commit is contained in:
207
maneuvers/approach.py
Normal file
207
maneuvers/approach.py
Normal file
@@ -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))))
|
||||
Reference in New Issue
Block a user