Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add dynamics_velocity_NoAir #23

Merged
merged 8 commits into from
May 27, 2024
Merged

Add dynamics_velocity_NoAir #23

Changes from 4 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
7af6b18
add dynamics_velocity _NoAir
A-Hayasaka May 21, 2024
ff4d41e
black
A-Hayasaka May 21, 2024
a8d2a16
Update constraints
A-Hayasaka May 22, 2024
9d3cc58
update equality_jac_dynamics_velocity
A-Hayasaka May 22, 2024
48dbc2f
add inequality_length (max_alpha, max_q, max_qalpha)
A-Hayasaka May 27, 2024
ac2c5a8
vectorize aerodynamic constraints
A-Hayasaka May 27, 2024
a81fdf9
vectorize dynamics jacobian calculation
A-Hayasaka May 27, 2024
50a69f8
remove profiler
A-Hayasaka May 27, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
149 changes: 91 additions & 58 deletions constraints.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,28 @@ def dynamics_velocity(
return acc_eci / units[2]


@jit(nopython=True)
def dynamics_velocity_NoAir(mass_e, pos_eci_e, quat_eci2body, param, units):
"""Equation of motion of velocity."""

mass = mass_e * units[0]
pos_eci = pos_eci_e * units[1]
acc_eci = np.zeros(pos_eci_e.shape)

thrust_vac = param[0]

for i in range(len(mass)):

thrust = thrust_vac
thrustdir_eci = quatrot(conj(quat_eci2body[i]), np.array([1.0, 0.0, 0.0]))
thrust_eci = thrustdir_eci * thrust
gravity_eci = gravity(pos_eci[i])

acc_eci[i] = gravity_eci + (thrust_eci) / mass[i]

return acc_eci / units[2]


@jit(nopython=True)
def dynamics_quaternion(quat_eci2body, u_e, unit_u):
"""Equation of motion of quaternion."""
Expand Down Expand Up @@ -454,22 +476,36 @@ def equality_dynamics_velocity(xdict, pdict, unitdict, condition):
ca = pdict["ca_table"]

lh = pdict["ps_params"][i]["D"].dot(vel_i_)
rh = (
dynamics_velocity(
mass_i_[1:],
pos_i_[1:],
vel_i_[1:],
quat_i_[1:],
t_nodes,
param,
wind,
ca,
units,
if param[2] == 0.0:
rh = (
dynamics_velocity_NoAir(
mass_i_[1:],
pos_i_[1:],
quat_i_[1:],
param,
units,
)
* (tf - to)
* unit_t
/ 2.0
)
else:
rh = (
dynamics_velocity(
mass_i_[1:],
pos_i_[1:],
vel_i_[1:],
quat_i_[1:],
t_nodes,
param,
wind,
ca,
units,
)
* (tf - to)
* unit_t
/ 2.0
)
* (tf - to)
* unit_t
/ 2.0
)
con.append((lh - rh).ravel())

return np.concatenate(con, axis=None)
Expand Down Expand Up @@ -533,30 +569,38 @@ def equality_jac_dynamics_velocity(xdict, pdict, unitdict, condition):
submat_vel[1::3, 1::3] = pdict["ps_params"][i]["D"]
submat_vel[2::3, 2::3] = pdict["ps_params"][i]["D"]

f_center = dynamics_velocity(
def dynamics(mass, pos, vel, quat, t):
if param[2] == 0.0:
return dynamics_velocity_NoAir(mass, pos, quat, param, units)
else:
return dynamics_velocity(
mass,
pos,
vel,
quat,
t,
param,
wind,
ca,
units,
)

f_center = dynamics(
mass_i_[1:],
pos_i_[1:],
vel_i_[1:],
quat_i_[1:],
t_nodes,
param,
wind,
ca,
units,
)

for j in range(n):
mass_i_[j + 1] += dx
f_p = dynamics_velocity(
f_p = dynamics(
mass_i_[1:],
pos_i_[1:],
vel_i_[1:],
quat_i_[1:],
t_nodes,
param,
wind,
ca,
units,
)
mass_i_[j + 1] -= dx
rh_mass = (
Expand All @@ -568,16 +612,12 @@ def equality_jac_dynamics_velocity(xdict, pdict, unitdict, condition):

for k in range(3):
pos_i_[j + 1, k] += dx
f_p = dynamics_velocity(
f_p = dynamics(
mass_i_[1:],
pos_i_[1:],
vel_i_[1:],
quat_i_[1:],
t_nodes,
param,
wind,
ca,
units,
)
pos_i_[j + 1, k] -= dx
rh_pos = (
Expand All @@ -589,42 +629,35 @@ def equality_jac_dynamics_velocity(xdict, pdict, unitdict, condition):
jac["position"]["coo"][1].extend([(a + i + j + 1) * 3 + k] * 3)
jac["position"]["coo"][2].extend(rh_pos.tolist())

for k in range(3):
vel_i_[j + 1, k] += dx
f_p = dynamics_velocity(
mass_i_[1:],
pos_i_[1:],
vel_i_[1:],
quat_i_[1:],
t_nodes,
param,
wind,
ca,
units,
)
vel_i_[j + 1, k] -= dx
submat_vel[j * 3, (j + 1) * 3 + k] += (
-(f_p[j, 0] - f_center[j, 0]) / dx * (tf - to) * unit_t / 2.0
) # rh acc_x vel
submat_vel[j * 3 + 1, (j + 1) * 3 + k] += (
-(f_p[j, 1] - f_center[j, 1]) / dx * (tf - to) * unit_t / 2.0
) # rh acc_x vel
submat_vel[j * 3 + 2, (j + 1) * 3 + k] += (
-(f_p[j, 2] - f_center[j, 2]) / dx * (tf - to) * unit_t / 2.0
) # rh acc_x vel
if param[2] > 0.0:
for k in range(3):
vel_i_[j + 1, k] += dx
f_p = dynamics(
mass_i_[1:],
pos_i_[1:],
vel_i_[1:],
quat_i_[1:],
t_nodes,
)
vel_i_[j + 1, k] -= dx
submat_vel[j * 3, (j + 1) * 3 + k] += (
-(f_p[j, 0] - f_center[j, 0]) / dx * (tf - to) * unit_t / 2.0
) # rh acc_x vel
submat_vel[j * 3 + 1, (j + 1) * 3 + k] += (
-(f_p[j, 1] - f_center[j, 1]) / dx * (tf - to) * unit_t / 2.0
) # rh acc_x vel
submat_vel[j * 3 + 2, (j + 1) * 3 + k] += (
-(f_p[j, 2] - f_center[j, 2]) / dx * (tf - to) * unit_t / 2.0
) # rh acc_x vel

for k in range(4):
quat_i_[j + 1, k] += dx
f_p = dynamics_velocity(
f_p = dynamics(
mass_i_[1:],
pos_i_[1:],
vel_i_[1:],
quat_i_[1:],
t_nodes,
param,
wind,
ca,
units,
)
quat_i_[j + 1, k] -= dx
rh_quat = (
Expand Down
Loading