Commit da29e297 authored by li213's avatar li213
Browse files

add spacecraft rendezvous example

parent 5b8f1eea
from rendezvous import *
\ No newline at end of file
from scipy.integrate import odeint
import numpy as np
def ProxA_dynamics(x,_):
xp, yp, xd, yd = x
xp_dot = xd
yp_dot= yd
xd_dot= -2.89995083970656*xd - 0.0576765518445905*xp + 0.00877200894463775*yd + 0.000200959896519766*yp - (1.43496e+18*xp + 6.050365344e+25)*pow(pow(yp, 2) + pow(xp + 42164000, 2), -1.5) + 807.153595726846
yd_dot= -0.00875351105536225*xd - 0.000174031357370456*xp - 2.90300269286856*yd - 1.43496e+18*yp*pow(pow(yp, 2) + pow(xp + 42164000, 2), -1.5) - 0.0664932019993982*yp
return [xp_dot, yp_dot, xd_dot, yd_dot]
def ProxB_dynamics(x,t):
xp, yp, xd, yd = x
xp_dot= xd
yp_dot= yd
xd_dot= -19.2299795908647*xd - 0.576076729033652*xp + 0.00876275931760007*yd + 0.000262486079431672*yp - (1.43496e+18*xp + 6.050365344e+25)*pow(pow(yp, 2) + pow(xp + 42164000, 2), -1.5) + 807.153595726846
yd_dot= -0.00876276068239993*xd - 0.000262486080737868*xp - 19.2299765959399*yd - 1.43496e+18*yp*pow(pow(yp, 2) + pow(xp + 42164000, 2), -1.5) - 0.575980743701182*yp
return [xp_dot, yp_dot, xd_dot, yd_dot]
def Passive_dynamics(x,t):
xp, yp, xd, yd = x
xp_dot = xd
yp_dot = yd
xd_dot = 0.0000575894721132000*xp+0.00876276*yd
yd_dot = -0.00876276*xd
return [xp_dot, yp_dot, xd_dot, yd_dot]
def simulate(init, time_bound, time_step, dynamics):
num_step = int(time_bound/time_step)
x = init
trace = [[0]+x]
t = 0
for i in range(num_step):
x = odeint(dynamics, x, [0,time_step])[-1].tolist()
t += time_step
trace.append([t]+x)
return np.array(trace)
def TC_Simulate(Mode,initialCondition,time_bound):
time_step = 0.01
init = [float(elem) for elem in initialCondition]
if Mode == "ProxA":
trace = simulate(init, time_bound, time_step, ProxA_dynamics)
elif Mode == "ProxB":
trace = simulate(init, time_bound, time_step, ProxB_dynamics)
elif Mode == "Passive":
trace = simulate(init, time_bound, time_step, Passive_dynamics)
else:
raise ValueError(f"Mode {Mode} not found")
return trace
{
"vertex":["ProxA","ProxB","Passive"],
"variables":["xp","yp","xd","yd"],
"edge":[[0,1],[0,2],[1,2]],
"guards":[
"And(yp>=-100, xp+yp>=-141.1, xp>=-100, yp-xp<=141.1, yp<=100, xp+yp<=141.1, xp<=100, yp-xp>=-141.1)",
"And(t>=120)",
"And(t>=120)"
],
"initialVertex":0,
"resets":["", "",""],
"initialSet":[[-925,-425,0,0],[-875,-375,0,0]],
"unsafeSet":"",
"timeHorizon":200,
"directory":"examples/rendezvous"
}
\ No newline at end of file
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment