def desired_attitude(self,
time,
alpha=2 * np.pi / 100,
axis=np.array([0, 1, 0])):
"""Desired attitude trajectory
This function will output a desired attitude trajectory. The controller will use this
trajectory in it's computations. The outputs will be the desired attitude matrix and
the desired angular velocity:
Outputs:
Rd_sc2int - 3x3 array defining the transformation from the spacecraft frame to the
inertial frame
w_sc2int - 3 array defining the angular velocity of the spacecraft frame with respect
to the inertial frame and defined in the spacecraft fixed frame
"""
Rd = scipy.linalg.expm(alpha * time * attitude.hat_map(axis))
Rd_dot = alpha * attitude.hat_map(axis).dot(
scipy.linalg.expm(alpha * time * attitude.hat_map(axis)))
ang_vel_d = attitude.vee_map(Rd.T.dot(Rd_dot))
ang_vel_d_dot = np.zeros_like(ang_vel_d)
return (Rd, Rd_dot, ang_vel_d, ang_vel_d_dot)
def test_expm_rot2_derivative(self):
axis = np.array([0, 1, 0])
R_dot = self.alpha * attitude.hat_map(axis).dot(
scipy.linalg.expm(self.alpha * self.t * attitude.hat_map(axis)))
R2_dot = attitude.rot2(self.alpha * self.t).dot(
attitude.hat_map(self.alpha * axis))
np.testing.assert_almost_equal(R_dot, R2_dot)
def test_relative_moment_of_inertia(self):
R = self.R.reshape((3, 3))
Jr = R.dot(self.dum.J).dot(R.T)
Jdr = R.dot(self.dum.Jd).dot(R.T)
np.testing.assert_array_almost_equal(
attitude.hat_map(Jr.dot(self.ang_vel)),
attitude.hat_map(self.ang_vel).dot(Jdr) +
Jdr.dot(attitude.hat_map(self.ang_vel)))
def eoms_inertial(self, state, t, ast):
"""Inertial dumbbell equations of motion about an asteroid
Inputs:
t -
state -
ast - Asteroid class object holding the asteroid gravitational model and
other useful parameters
"""
# unpack the state
pos = state[0:
3] # location of the center of mass in the inertial frame
vel = state[3:6] # vel of com in inertial frame
R = np.reshape(state[6:15], (3, 3)) # sc body frame to inertial frame
ang_vel = state[
15:
18] # angular velocity of sc wrt inertial frame defined in body frame
Ra = attitude.rot3(ast.omega * t,
'c') # asteroid body frame to inertial frame
# unpack parameters for the dumbbell
J = self.J
rho1 = self.zeta1
rho2 = self.zeta2
# position of each mass in the asteroid frame
z1 = Ra.T.dot(pos + R.dot(rho1))
z2 = Ra.T.dot(pos + R.dot(rho2))
z = Ra.T.dot(pos) # position of COM in asteroid frame
# compute the potential at this state
(U1, U1_grad, U1_grad_mat, U1laplace) = ast.polyhedron_potential(z1)
(U2, U2_grad, U2_grad_mat, U2laplace) = ast.polyhedron_potential(z2)
F1 = self.m1 * Ra.dot(U1_grad)
F2 = self.m2 * Ra.dot(U2_grad)
M1 = self.m1 * attitude.hat_map(rho1).dot(R.T.dot(Ra).dot(U1_grad))
M2 = self.m2 * attitude.hat_map(rho2).dot(R.T.dot(Ra).dot(U2_grad))
# M1 = np.zeros(3)
# M2 = np.zeros_like(3)
pos_dot = vel
vel_dot = 1 / (self.m1 + self.m2) * (F1 + F2)
R_dot = R.dot(attitude.hat_map(ang_vel)).reshape(9)
ang_vel_dot = np.linalg.inv(J).dot(-np.cross(ang_vel, J.dot(ang_vel)) +
M1 + M2)
statedot = np.hstack((pos_dot, vel_dot, R_dot, ang_vel_dot))
return statedot
def translation_controller_asteroid(time, state, ext_force, dum, ast,
des_tran_tuple):
"""SE(3) Translational Controller
This function determines the control input for the dumbbell with the equations
of motion defined in the asteroid fixed frame.
Use this with eoms_relative
Parameters
----------
time : float
Current time for simulation which is used in the desired attitude trajectory
state : array_like (18,)
numpy array defining the state of the dumbbell
position - position of the center of mass wrt to the asteroid com
and defined in the asteroid frame (3,)
velocity - velocity of the center of mass wrt to teh asteroid com
and defined in the asteroid frame (3,)
R_b2i - rotation matrix which transforms vectors from the body
frame to the asteroid frame (9,)
angular_velocity - angular velocity of the body frame with respect
to the inertial frame and defined in the asteroid frame (3,)
ext_force : array_like (3,)
External force in the asteroid frame
dum : Dumbbell Instance
Instance of a dumbbell which defines the shape and MOI
ast : Asteroid Instance
Holds the asteroid model and polyhedron potential
des_tran_tuple : Desired translational tuple defined with respect to the asteroid frame
des_tran_tuple [0] - Rd desired attitude
des_tran_tuple [1] - Rd_dot desired attitude derivative
des_tran_tuple [2] - ang_vel_d angular velocity
des_tran_tuple [3] - ang_vel_d_dot angular velocity desired derivative
"""
# extract the state
pos = state[0:3] # location of the center of mass in the asteroid frame
vel = state[3:6] # vel of com in asteroid frame
R = np.reshape(state[6:15], (3, 3)) # sc body frame to asteroid frame
ang_vel = state[
15:
18] # angular velocity of sc wrt inertial frame defined in asteroid frame
m = dum.m1 + dum.m2
Wa = ast.omega * np.array([0, 0, 1]) # angular velocity vector of asteroid
x_des = des_tran_tuple[0]
xd_des = des_tran_tuple[1]
xdd_des = des_tran_tuple[2]
# compute the error
ex = pos - x_des
ev = vel - xd_des
# compute the control
u_f = -dum.kx * ex - dum.kv * ev - ext_force + m * xdd_des + m * attitude.hat_map(
Wa).dot(vel)
return u_f
def inertial_energy(self, time, state, ast):
"""Compute the kinetic and potential energy of the dumbbell given the current state
Input:
vel - nx3 velocity in inertial frame (km/sec)
ang_vel - nx3 angular velocity of dumbbell frame wrt to inertial frame expressed in dumbbell frame (rad/sec)
Outputs:
T - nx1 kinetic energy array which should be the same length as state input
"""
# some constants
m = self.m1 + self.m2 # total mass of dumbbell in kg
Jd = self.Jd
KE = np.zeros(time.shape[0])
PE = np.zeros(time.shape[0])
for ii in range(time.shape[0]):
pos = state[
ii,
0:3] # location of the center of mass in the inertial frame
vel = state[ii, 3:6] # vel of com in inertial frame
R = np.reshape(state[ii, 6:15],
(3, 3)) # sc body frame to inertial frame
ang_vel = state[
ii, 15:
18] # angular velocity of sc wrt inertial frame defined in body frame
Ra = attitude.rot3(ast.omega * time[ii],
'c') # asteroid body frame to inertial frame
# position of each mass in the inertial frame
z1 = Ra.T.dot(pos + R.dot(self.zeta1))
z2 = Ra.T.dot(pos + R.dot(self.zeta2))
(U1, _, _, _) = ast.polyhedron_potential(z1)
(U2, _, _, _) = ast.polyhedron_potential(z2)
PE[ii] = -self.m1 * U1 - self.m2 * U2
KE[ii] = 1.0 / 2 * m * vel.dot(vel) + 1.0 / 2 * np.trace(
attitude.hat_map(ang_vel).dot(Jd).dot(
attitude.hat_map(ang_vel).T))
return KE, PE
def relative_energy(self, time, state, ast):
"""Compute the KE and PE of the relative equations of motion
"""
m = self.m1 + self.m2
J1 = self.J
Jd = self.Jd
KE = np.zeros(time.shape[0])
PE = np.zeros(time.shape[0])
for ii in range(time.shape[0]):
pos = state[
ii,
0:3] # location of the COM wrt asteroid in the asteroid frame
vel = state[ii, 3:6] # vel of COM wrt asteroid in asteroid frame
R = np.reshape(state[ii, 6:15],
(3, 3)) # sc body frame to asteroid frame
ang_vel = state[
ii, 15:
18] # angular velocity of sc wrt inertial frame defined in asteroid frame
# position of each mass in the inertial frame
z1 = pos + R.dot(self.zeta1)
z2 = pos + R.dot(self.zeta2)
(U1, _, _, _) = ast.polyhedron_potential(z1)
(U2, _, _, _) = ast.polyhedron_potential(z2)
Jdr = R.dot(Jd).dot(R.T)
PE[ii] = -self.m1 * U1 - self.m2 * U2
KE[ii] = 1 / 2 * m * vel.dot(vel) + 1 / 2 * np.trace(
attitude.hat_map(ang_vel).dot(Jdr).dot(
attitude.hat_map(ang_vel).T))
return KE, PE
def eoms_inertial_ode(self, t, state):
"""
EOMS of spacecraft with respect to Earth in the ECI frame
"""
pos = state[0:3]
vel = state[3:6]
R = np.reshape(state[6:15], (3, 3)) # rotation from body frame to ECI
w = state[15:18] # ang vel of body frame wrt ECI in body frame
m_sat = self.m
J = self.J
accel_gravity = propogator.accel_twobody(m_sat, constants.earth.mass,
pos, constants.G)
ext_moment = np.zeros(3)
pos_dot = vel
vel_dot = accel_gravity
R_dot = R.dot(attitude.hat_map(w))
w_dot = np.linalg.inv(J).dot(ext_moment -
attitude.hat_map(w).dot(J).dot(w))
state_dot = np.concatenate((pos_dot, vel_dot, R_dot.reshape(9), w_dot))
return state_dot
class TestHamiltonRelativeTransform():
time = np.array([0])
ast = asteroid.Asteroid(name='castalia', num_faces=64)
dum = dumbbell.Dumbbell()
inertial_pos = np.array([1, 1, 1])
inertial_vel = np.random.rand(3) + att.hat_map(
ast.omega * np.array([0, 0, 1])).dot(inertial_pos)
R_sc2int = att.rot2(np.pi / 2)
R_ast2int = att.rot3(time * ast.omega)
body_ang_vel = np.random.rand(3)
initial_lin_mom = (dum.m1 + dum.m2) * (inertial_vel)
initial_ang_mom = R_sc2int.dot(dum.J).dot(body_ang_vel)
initial_ham_state = np.hstack(
(inertial_pos, initial_lin_mom, R_ast2int.dot(R_sc2int).reshape(9),
initial_ang_mom))
inertial_state = transform.eoms_hamilton_relative_to_inertial(
time, initial_ham_state, ast, dum)
def test_eoms_hamilton_relative_to_inertial_scalar_pos(self):
np.testing.assert_almost_equal(self.inertial_pos,
self.inertial_state[0:3])
def test_eoms_hamilton_relative_to_inertial_scalar_vel(self):
np.testing.assert_almost_equal(self.inertial_vel,
self.inertial_state[3:6])
def test_eoms_hamilton_relative_to_inertial_scalar_att(self):
np.testing.assert_almost_equal(self.R_sc2int.reshape(9),
self.inertial_state[6:15])
def test_eoms_hamilton_relative_to_inertial_scalar_ang_vel(self):
np.testing.assert_almost_equal(self.R_sc2int.dot(self.body_ang_vel),
self.inertial_state[15:18])
def attitude_land_controller(time, state, ext_moment, dum, ast):
"""Landing controller to just stay pointed at the asteroid
This function is probably not necessary since many others do the same thing
"""
# extract the state
pos = state[0:3] # location of the center of mass in the inertial frame
vel = state[3:6] # vel of com in inertial frame
R = np.reshape(state[6:15], (3, 3)) # sc body frame to inertial frame
ang_vel = state[
15:
18] # angular velocity of sc wrt inertial frame defined in body frame
# compute the desired attitude command
Rd, Rd_dot, ang_vel_d, ang_vel_d_dot = body_fixed_pointing_attitude(
time, state)
# determine error between command and current state
eR = 1 / 2 * attitude.vee_map(Rd.T.dot(R) - R.T.dot(Rd))
eW = ang_vel - R.T.dot(Rd).dot(ang_vel_d)
# compute attitude input
u_m = (-dum.kR * eR - dum.kW * eW + np.cross(ang_vel, dum.J.dot(ang_vel)) -
dum.J.dot(
attitude.hat_map(ang_vel).dot(R.T).dot(Rd).dot(ang_vel_d) -
R.T.dot(Rd).dot(ang_vel_d_dot)) - ext_moment)
return u_m
def attitude_controller(self, time, state, ext_moment):
r"""Geometric attitude controller on SO(3)
This function will determine an attitude control input for a rigid spacecraft around an asteroid.
The function is setup to work for a vehicle defined in the inertial frame relative to an asteroid.
Parameters
----------
self : dumbbell instance
Instance of dumbbell class with all of it's parameters
time : float
Current time for simulation which is used in the desired attitude trajectory
state : array_like (18,)
numpy array defining the state of the dumbbell
position - position of the center of mass wrt to the inertial frame
and defined in the inertial frame (3,)
velocity - velocity of the center of mass wrt to teh inertial frame
and defined in the inertial frame (3,)
R_b2i - rotation matrix which transforms vectors from the body
frame to the inertial frame (9,)
angular_velocity - angular velocity of the body frame with respect
to the inertial frame and defined in the body frame (3,)
ext_moment : array_like (3,)
External moment in the body fixed frame
Returns
-------
u_m : array_like (3,)
Body fixed control moment
Author
------
Shankar Kulumani GWU [email protected]
References
----------
.. [1] LEE, Taeyoung, LEOK, Melvin y MCCLAMROCH, N Harris. "Control of
Complex Maneuvers for a Quadrotor UAV Using Geometric Methods on Se
(3)". arXiv preprint arXiv:1003.2005. 2010,
Examples
--------
"""
# extract the state
pos = state[0:
3] # location of the center of mass in the inertial frame
vel = state[3:6] # vel of com in inertial frame
R = np.reshape(state[6:15], (3, 3)) # sc body frame to inertial frame
ang_vel = state[
15:
18] # angular velocity of sc wrt inertial frame defined in body frame
# compute the desired attitude command
Rd, Rd_dot, ang_vel_d, ang_vel_d_dot = self.desired_attitude(time)
# determine error between command and current state
eR = 1 / 2 * attitude.vee_map(Rd.T.dot(R) - R.T.dot(Rd))
eW = ang_vel - R.T.dot(Rd).dot(ang_vel_d)
# compute attitude input
u_m = (-self.kR * eR - self.kW * eW +
np.cross(ang_vel, self.J.dot(ang_vel)) - self.J.dot(
attitude.hat_map(ang_vel).dot(R.T).dot(Rd).dot(ang_vel_d) -
R.T.dot(Rd).dot(ang_vel_d_dot)) - ext_moment)
return u_m
def eoms_controlled_inertial_refinement_pybind(t, state, true_ast, dum,
complete_controller,
est_ast_rmesh, est_ast,
desired_landing_site):
"""We'll hover over the landing site and look at
all the points in view
This function must be used with scipy.integrate.ode class instead of the
more convienent scipe.integrate.odeint. In addition, we can control the
dumbbell given full state feedback. This uses several C++ functions which
are exposed to Python using PyBind11
Arguments
---------
t : current simulation time step
state : (18, ) numpy array of the state
pos - (3,) position of the dumbbell with respect to the
asteroid center of mass and expressed in the inertial frame
vel - (3,) velocity of the dumbbell with respect to the
asteroid center of mass and expressed in the inertial frame
R - (9,) attitude of the dumbbell with defines the
transformation of a vector in the dumbbell frame to the
inertial frame ang_vel - (3,) angular velocity of the dumbbell
with respect to the inertial frame and represented in the
dumbbell frame
ast : asteroid object (from C++ bindings)
dum : dumbbell object (from Python)
complete_controller : controller object (from C++)
"""
# unpack the state
pos = state[0:3] # location of the center of mass in the inertial frame
vel = state[3:6] # vel of com in inertial frame
R = np.reshape(state[6:15], (3, 3)) # sc body frame to inertial frame
ang_vel = state[
15:
18] # angular velocity of sc wrt inertial frame defined in body frame
Ra = true_ast.rot_ast2int(t) # asteroid body frame to inertial frame
# unpack parameters for the dumbbell
J = dum.J
rho1 = dum.zeta1
rho2 = dum.zeta2
# position of each mass in the asteroid frame
z1 = Ra.T.dot(pos + R.dot(rho1))
z2 = Ra.T.dot(pos + R.dot(rho2))
z = Ra.T.dot(pos) # position of COM in asteroid frame
# gradient and potential at this state
true_ast.polyhedron_potential(z1)
U1 = true_ast.get_potential()
U1_grad = true_ast.get_acceleration()
true_ast.polyhedron_potential(z2)
U2 = true_ast.get_potential()
U2_grad = true_ast.get_acceleration()
F1 = dum.m1 * Ra.dot(U1_grad)
F2 = dum.m2 * Ra.dot(U2_grad)
M1 = dum.m1 * attitude.hat_map(rho1).dot(R.T.dot(Ra).dot(U1_grad))
M2 = dum.m2 * attitude.hat_map(rho2).dot(R.T.dot(Ra).dot(U2_grad))
# compute the external force and moment using the asteroid estimate
est_ast.polyhedron_potential(z1)
U1_est = est_ast.get_potential()
U1_grad_est = est_ast.get_acceleration()
est_ast.polyhedron_potential(z2)
U2_est = est_ast.get_potential()
U2_grad_est = est_ast.get_acceleration()
F1_est = dum.m1 * Ra.dot(U1_grad_est)
F2_est = dum.m2 * Ra.dot(U2_grad_est)
M1_est = dum.m1 * attitude.hat_map(rho1).dot(R.T.dot(Ra).dot(U1_grad_est))
M2_est = dum.m2 * attitude.hat_map(rho2).dot(R.T.dot(Ra).dot(U2_grad_est))
# compute the desired states for exploration
complete_controller.refinement(t, state, est_ast_rmesh, est_ast,
desired_landing_site)
# Need to convert to the inertial frame for use in the controller
des_att_tuple = (Ra.dot(complete_controller.get_Rd()),
Ra.dot(complete_controller.get_Rd_dot()),
Ra.dot(complete_controller.get_ang_vel_d()),
Ra.dot(complete_controller.get_ang_vel_d_dot()))
des_tran_tuple = (Ra.dot(complete_controller.get_posd()),
Ra.dot(complete_controller.get_veld()),
Ra.dot(complete_controller.get_acceld()))
# need to pass in the estimated external torque and moment
u_m = controller.attitude_controller(t, state, M1_est + M2_est, dum,
est_ast, des_att_tuple)
u_f = controller.translation_controller(t, state, F1_est + F2_est, dum,
est_ast, des_tran_tuple)
pos_dot = vel
vel_dot = 1 / (dum.m1 + dum.m2) * (F1 + F2 + u_f)
R_dot = R.dot(attitude.hat_map(ang_vel)).reshape(9)
ang_vel_dot = np.linalg.inv(J).dot(-np.cross(ang_vel, J.dot(ang_vel)) +
M1 + M2 + u_m)
state_dot = np.hstack((pos_dot, vel_dot, R_dot, ang_vel_dot))
return state_dot
class TestInertialandRelativeEOMS():
"""Compare the inertial and relative eoms against one another
"""
RelTol = 1e-9
AbsTol = 1e-9
ast_name = 'castalia'
num_faces = 64
tf = 1e2
num_steps = 1e2
time = np.linspace(0, tf, num_steps)
periodic_pos = np.array(
[1.495746722510590, 0.000001002669660, 0.006129720493607])
periodic_vel = np.array(
[0.000000302161724, -0.000899607989820, -0.000000013286327])
ast = asteroid.Asteroid(ast_name, num_faces)
dum = dumbbell.Dumbbell(m1=500, m2=500, l=0.003)
# set initial state for inertial EOMs
initial_pos = periodic_pos # km for center of mass in body frame
initial_vel = periodic_vel + att.hat_map(
ast.omega * np.array([0, 0, 1])).dot(initial_pos)
initial_R = att.rot2(np.pi / 2).reshape(
9) # transforms from dumbbell body frame to the inertial frame
initial_w = np.array([0.01, 0.01, 0.01])
initial_state = np.hstack((initial_pos, initial_vel, initial_R, initial_w))
i_state = integrate.odeint(dum.eoms_inertial,
initial_state,
time,
args=(ast, ),
atol=AbsTol,
rtol=RelTol)
# (i_time, i_state) = idriver.eom_inertial_driver(initial_state, time, ast, dum, AbsTol=1e-9, RelTol=1e-9)
initial_lin_mom = (dum.m1 + dum.m2) * (periodic_vel + att.hat_map(
ast.omega * np.array([0, 0, 1])).dot(initial_pos))
initial_ang_mom = initial_R.reshape((3, 3)).dot(dum.J).dot(initial_w)
initial_ham_state = np.hstack(
(initial_pos, initial_lin_mom, initial_R, initial_ang_mom))
rh_state = integrate.odeint(dum.eoms_hamilton_relative,
initial_ham_state,
time,
args=(ast, ),
atol=AbsTol,
rtol=RelTol)
# now convert both into the inertial frame
istate_ham = transform.eoms_hamilton_relative_to_inertial(
time, rh_state, ast, dum)
istate_int = transform.eoms_inertial_to_inertial(time, i_state, ast, dum)
# also convert both into the asteroid frame and compare
astate_ham = transform.eoms_hamilton_relative_to_asteroid(
time, rh_state, ast, dum)
astate_int = transform.eoms_inertial_to_asteroid(time, i_state, ast, dum)
def test_inertial_frame_comparison_pos(self):
np.testing.assert_array_almost_equal(self.istate_ham[:, 0:3],
self.istate_int[:, 0:3])
def test_inertial_frame_comparison_vel(self):
np.testing.assert_array_almost_equal(self.istate_ham[:, 3:6],
self.istate_int[:, 3:6])
def test_inertial_frame_comparison_att(self):
np.testing.assert_array_almost_equal(self.istate_ham[:, 6:15],
self.istate_int[:, 6:15])
def test_inertial_frame_comparison_ang_vel(self):
np.testing.assert_array_almost_equal(self.istate_ham[:, 15:18],
self.istate_int[:, 15:18])
def test_asteroid_frame_comparison_pos(self):
np.testing.assert_array_almost_equal(self.astate_ham[:, 0:3],
self.astate_int[:, 0:3])
def test_asteroid_frame_comparison_vel(self):
np.testing.assert_array_almost_equal(self.astate_ham[:, 3:6],
self.astate_int[:, 3:6])
def test_asteroid_frame_comparison_att(self):
np.testing.assert_array_almost_equal(self.astate_ham[:, 6:15],
self.astate_int[:, 6:15])
def test_asteroid_frame_comparison_ang_vel(self):
np.testing.assert_array_almost_equal(self.astate_ham[:, 15:18],
self.astate_int[:, 15:18])
def eoms_controlled_blender(t, state, dum, ast):
"""Inertial dumbbell equations of motion about an asteroid
This method must be used with the scipy.integrate.ode class instead of the
more convienent scipy.integrate.odeint. In addition, we can control the
dumbbell given full state feedback. Blender is used to generate imagery
during the simulation.
Inputs:
t - Current simulation time step
state - (18,) array which defines the state of the vehicle
pos - (3,) position of the dumbbell with respect to the
asteroid center of mass and expressed in the inertial frame
vel - (3,) velocity of the dumbbell with respect to the
asteroid center of mass and expressed in the inertial frame
R - (9,) attitude of the dumbbell with defines the
transformation of a vector in the dumbbell frame to the
inertial frame ang_vel - (3,) angular velocity of the dumbbell
with respect to the inertial frame and represented in the
dumbbell frame
ast - Asteroid class object holding the asteroid gravitational
model and other useful parameters
"""
# unpack the state
pos = state[0:3] # location of the center of mass in the inertial frame
vel = state[3:6] # vel of com in inertial frame
R = np.reshape(state[6:15], (3, 3)) # sc body frame to inertial frame
ang_vel = state[
15:
18] # angular velocity of sc wrt inertial frame defined in body frame
Ra = attitude.rot3(ast.omega * t,
'c') # asteroid body frame to inertial frame
# unpack parameters for the dumbbell
J = dum.J
rho1 = dum.zeta1
rho2 = dum.zeta2
# position of each mass in the asteroid frame
z1 = Ra.T.dot(pos + R.dot(rho1))
z2 = Ra.T.dot(pos + R.dot(rho2))
z = Ra.T.dot(pos) # position of COM in asteroid frame
# compute the potential at this state
(U1, U1_grad, U1_grad_mat, U1laplace) = ast.polyhedron_potential(z1)
(U2, U2_grad, U2_grad_mat, U2laplace) = ast.polyhedron_potential(z2)
F1 = dum.m1 * Ra.dot(U1_grad)
F2 = dum.m2 * Ra.dot(U2_grad)
M1 = dum.m1 * attitude.hat_map(rho1).dot(R.T.dot(Ra).dot(U1_grad))
M2 = dum.m2 * attitude.hat_map(rho2).dot(R.T.dot(Ra).dot(U2_grad))
# generate image at this current state only at a specifc time
# blender.driver(pos, R, ast.omega * t, [5, 0, 1], 'test' + str.zfill(str(t), 4))
# use the imagery to figure out motion and pass to the controller instead
# of the true state
# calculate the desired attitude and translational trajectory
des_att_tuple = controller.body_fixed_pointing_attitude(t, state)
des_tran_tuple = controller.traverse_then_land_vertically(
t,
ast,
final_pos=[0.550, 0, 0],
initial_pos=[2.550, 0, 0],
descent_tf=3600)
# input trajectory and compute the control inputs
# compute the control input
u_m = controller.attitude_controller(t, state, M1 + M2, dum, ast,
des_att_tuple)
u_f = controller.translation_controller(t, state, F1 + F2, dum, ast,
des_tran_tuple)
pos_dot = vel
vel_dot = 1 / (dum.m1 + dum.m2) * (F1 + F2 + u_f)
R_dot = R.dot(attitude.hat_map(ang_vel)).reshape(9)
ang_vel_dot = np.linalg.inv(J).dot(-np.cross(ang_vel, J.dot(ang_vel)) +
M1 + M2 + u_m)
statedot = np.hstack((pos_dot, vel_dot, R_dot, ang_vel_dot))
return statedot
choices=[0, 1],
help=
"Choose which inertial energy mode to run. 0 - inertial energy, 1 - \Delta E behavior"
)
args = parser.parse_args()
print("Starting the simulation...")
# instantiate the asteroid and dumbbell objects
ast = asteroid.Asteroid(args.ast_name, args.num_faces)
dum = dumbbell.Dumbbell(m1=500, m2=500, l=0.003)
# initialize simulation parameters
time = np.linspace(0, args.tf, args.num_steps)
initial_pos = periodic_pos # km for center of mass in body frame
initial_vel = periodic_vel + attitude.hat_map(
ast.omega * np.array([0, 0, 1])).dot(initial_pos)
initial_R = attitude.rot2(0).reshape(
9) # transforms from dumbbell body frame to the inertial frame
initial_w = np.array([0.01, 0.01, 0.01])
initial_state = np.hstack((initial_pos, initial_vel, initial_R, initial_w))
# echo back all the input parameters
print("This will run a long simulation with the following parameters!")
print(" ast_name - %s" % args.ast_name)
print(" num_faces - %s" % args.num_faces)
print(" tf - %s" % args.tf)
print(" num_steps - %s" % args.num_steps)
print(" file_name - %s" % args.file_name)
print("")
if args.mode == 0:
(_, inertial_state, asteroid_state,
def test_hat_vee_map_inverse(self):
np.testing.assert_allclose(attitude.vee_map(attitude.hat_map(self.x)),
self.x)
def test_hat_map_zero(self):
np.testing.assert_allclose(
attitude.hat_map(self.x).dot(self.x), np.zeros(3))
def eoms_inertial_control(self, state, t, ast):
"""Inertial dumbbell equations of motion about an asteroid with body fixed control capability
Inputs:
t - Current simulation time step
state - (18,) array which defines the state of the vehicle
pos - (3,) position of the dumbbell with respect to the asteroid center of mass
and expressed in the inertial frame
vel - (3,) velocity of the dumbbell with respect to the asteroid center of mass
and expressed in the inertial frame
R - (9,) attitude of the dumbbell with defines the transformation of a vector in the
dumbbell frame to the inertial frame
ang_vel - (3,) angular velocity of the dumbbell with respect to the inertial frame
and represented in the dumbbell frame
ast - Asteroid class object holding the asteroid gravitational model and
other useful parameters
"""
# unpack the state
pos = state[0:
3] # location of the center of mass in the inertial frame
vel = state[3:6] # vel of com in inertial frame
R = np.reshape(state[6:15], (3, 3)) # sc body frame to inertial frame
ang_vel = state[
15:
18] # angular velocity of sc wrt inertial frame defined in body frame
Ra = attitude.rot3(ast.omega * t,
'c') # asteroid body frame to inertial frame
# unpack parameters for the dumbbell
J = self.J
rho1 = self.zeta1
rho2 = self.zeta2
# position of each mass in the asteroid frame
z1 = Ra.T.dot(pos + R.dot(rho1))
z2 = Ra.T.dot(pos + R.dot(rho2))
z = Ra.T.dot(pos) # position of COM in asteroid frame
# compute the potential at this state
(U1, U1_grad, U1_grad_mat, U1laplace) = ast.polyhedron_potential(z1)
(U2, U2_grad, U2_grad_mat, U2laplace) = ast.polyhedron_potential(z2)
F1 = self.m1 * Ra.dot(U1_grad)
F2 = self.m2 * Ra.dot(U2_grad)
M1 = self.m1 * attitude.hat_map(rho1).dot(R.T.dot(Ra).dot(U1_grad))
M2 = self.m2 * attitude.hat_map(rho2).dot(R.T.dot(Ra).dot(U2_grad))
# compute the control input
u_m = self.attitude_controller(t, state, M1 + M2)
u_f = self.translation_controller(t, state, F1 + F2)
pos_dot = vel
vel_dot = 1 / (self.m1 + self.m2) * (F1 + F2 + u_f)
R_dot = R.dot(attitude.hat_map(ang_vel)).reshape(9)
ang_vel_dot = np.linalg.inv(J).dot(-np.cross(ang_vel, J.dot(ang_vel)) +
M1 + M2 + u_m)
statedot = np.hstack((pos_dot, vel_dot, R_dot, ang_vel_dot))
return statedot
def eoms_hamilton_relative(self, state, t, ast):
"""Hamiltonian form of Relative EOMS defined in the rotating asteroid frame
This function defines the motion of a dumbbell spacecraft in orbit around an asteroid.
The EOMS are defined relative to the asteroid itself, which is in a state of constant rotation.
You need to use this function with scipy.integrate.odeint
Inputs:
t - current time of simulation (sec)
state - (18,) relative hamiltonian state of dumbbell with respect to asteroid
pos - state[0:3] in km position of the dumbbell with respect to the asteroid and defined in the asteroid fixed frame
lin_mom - state[3:6] in kg km/sec is the linear momentum of dumbbell wrt the asteroid and defined in the asteroid fixed frame
R - state[6:15] rotation matrix which converts vectors from the dumbbell frame to the asteroid frame
ang_mom - state[15:18] J rad/sec angular momentum of the dumbbell wrt inertial frame and defined in the asteroid frame
ast - asteroid object
Output:
state_dot - (18,) derivative of state. The order is the same as the input state.
"""
# unpack the state
pos = state[
0:3] # location of the COM of dumbbell in asteroid fixed frame
lin_mom = state[
3:
6] # lin_mom of com wrt to asteroid expressed in the asteroid fixed frame
R = np.reshape(
state[6:15],
(3, 3)) # sc body frame to asteroid body frame R = R_A^T R_1
ang_mom = state[
15:
18] # angular momentum of sc wrt inertial frame and expressed in asteroid fixed frame
Ra = attitude.rot3(ast.omega * t,
'c') # asteroid body frame to inertial frame
# unpack parameters for the dumbbell
m1 = self.m1
m2 = self.m2
m = m1 + m2
J = self.J
Jr = R.dot(J).dot(R.T)
Wa = ast.omega * np.array([0, 0, 1
]) # angular velocity vector of asteroid
# the position of each mass in the dumbbell body frame
rho1 = self.zeta1
rho2 = self.zeta2
# position of each mass in the asteroid frame
z1 = pos + R.dot(rho1)
z2 = pos + R.dot(rho2)
z = pos # position of COM in asteroid frame
# compute the potential at this state
(U1, U1_grad, U1_grad_mat, U1laplace) = ast.polyhedron_potential(z1)
(U2, U2_grad, U2_grad_mat, U2laplace) = ast.polyhedron_potential(z2)
# force due to each mass expressed in asteroid body frame
F1 = m1 * U1_grad
F2 = m2 * U2_grad
M1 = m1 * attitude.hat_map(R.dot(rho1)).dot(U1_grad)
M2 = m2 * attitude.hat_map(R.dot(rho2)).dot(U2_grad)
vel = lin_mom / (m1 + m2)
w = np.linalg.inv(Jr).dot(ang_mom)
# state derivatives
pos_dot = vel - attitude.hat_map(Wa).dot(pos)
lin_mom_dot = F1 + F2 - attitude.hat_map(Wa).dot(lin_mom)
R_dot = attitude.hat_map(w).dot(R) - attitude.hat_map(Wa).dot(R)
R_dot = R_dot.reshape(9)
ang_mom_dot = M1 + M2 - attitude.hat_map(Wa).dot(ang_mom)
state_dot = np.hstack((pos_dot, lin_mom_dot, R_dot, ang_mom_dot))
return state_dot
def attitude_controller_asteroid(time, state, ext_moment, dum, ast,
des_att_tuple):
r"""Geometric attitude controller on SO(3)
This function will determine an attitude control input for a rigid
spacecraft around an asteroid.
The function is setup to work for a vehicle defined in the asteroid frame
and the control input is assumed to be defined in the asteroid fixed frame.
Parameters
----------
time : float
Current time for simulation which is used in the desired attitude trajectory
state : array_like (18,)
numpy array defining the state of the dumbbell
position - position of the center of mass wrt to the asteroid com
and defined in the asteroid frame (3,)
velocity - velocity of the center of mass wrt to teh asteroid com
and defined in the asteroid frame (3,)
R_b2i - rotation matrix which transforms vectors from the body
frame to the asteroid frame (9,)
angular_velocity - angular velocity of the body frame with respect
to the inertial frame and defined in the asteroid frame (3,)
ext_moment : array_like (3,)
External moment in the asteroid fixed frame
dum : Dumbbell Instance
Instance of a dumbbell which defines the shape and MOI
ast : Asteroid Instance
Holds the asteroid model and polyhedron potential
des_att_tuple : Desired attitude tuple for asteroid frame dyanmics
des_att_tuple[0] - Rd desired attitude
des_att_tuple[1] - Rd_dot desired attitude derivative
des_att_tuple[2] - ang_vel_d angular velocity
des_att_tuple[3] - ang_vel_d_dot angular velocity desired derivative
Returns
-------
u_m : array_like (3,)
asteroid fixed control moment
Author
------
Shankar Kulumani GWU [email protected]
References
----------
.. [1] LEE, Taeyoung, LEOK, Melvin y MCCLAMROCH, N Harris. "Control of
Complex Maneuvers for a Quadrotor UAV Using Geometric Methods on Se
(3)". arXiv preprint arXiv:1003.2005. 2010,
Examples
--------
"""
# extract the state
pos = state[0:3] # location of the center of mass in the asteroid frame
vel = state[3:6] # vel of com in asteroid frame
R = np.reshape(state[6:15], (3, 3)) # sc body frame to asteriod frame
ang_vel = state[
15:
18] # angular velocity of sc wrt inertial frame defined in asteriod frame
J = dum.J
Jr = R.dot(J).dot(R.T)
Wa = ast.omega * np.array([0, 0, 1]) # angular velocity vector of asteroid
Rd = des_att_tuple[0]
Rd_dot = des_att_tuple[1]
ang_vel_d = des_att_tuple[2]
ang_vel_d_dot = des_att_tuple[3]
# determine error between command and current state
eR = 1 / 2 * attitude.vee_map(Rd.T.dot(R) - R.T.dot(Rd))
eW = ang_vel - R.T.dot(Rd).dot(ang_vel_d)
# compute attitude input
u_m = (-dum.kR * eR - dum.kW * eW +
Jr.dot(attitude.hat_map(Wa).dot(ang_vel)) - Jr.dot(
attitude.hat_map(ang_vel).dot(R.T).dot(Rd).dot(ang_vel_d) -
R.T.dot(Rd).dot(ang_vel_d_dot)) - ext_moment)
return u_m
def test_expm_rot3_equivalent(self):
R = scipy.linalg.expm(self.angle *
attitude.hat_map(np.array([0, 0, 1])))
R3 = attitude.rot3(self.angle)
np.testing.assert_almost_equal(R.T.dot(R3), np.eye(3, 3))
def eoms_controlled_land_pybind(t, state, true_ast, dum, est_ast,
desired_asteroid_pos, t0, initial_position):
"""Inertial dumbbell equations of motion about an asteroid
This method must be used with the scipy.integrate.ode class instead of the
more convienent scipy.integrate.odeint. In addition, we can control the
dumbbell given full state feedback. Blender is used to generate imagery
during the simulation.
The spacecraft will move horizontally for the first 3600 sec to a positon
[2.550, 0, 0] in the asteroid (and inertial) frame, then descend vertically
in the asteroid frame.
Inputs:
t - Current simulation time step
state - (18,) array which defines the state of the vehicle
pos - (3,) position of the dumbbell with respect to the
asteroid center of mass and expressed in the inertial frame
vel - (3,) velocity of the dumbbell with respect to the
asteroid center of mass and expressed in the inertial frame
R - (9,) attitude of the dumbbell with defines the
transformation of a vector in the dumbbell frame to the
inertial frame ang_vel - (3,) angular velocity of the dumbbell
with respect to the inertial frame and represented in the
dumbbell frame
ast - Asteroid class object holding the asteroid gravitational
model and other useful parameters
"""
# unpack the state
pos = state[0:3] # location of the center of mass in the inertial frame
vel = state[3:6] # vel of com in inertial frame
R = np.reshape(state[6:15], (3, 3)) # sc body frame to inertial frame
ang_vel = state[
15:
18] # angular velocity of sc wrt inertial frame defined in body frame
Ra = est_ast.rot_ast2int(t) # asteroid body frame to inertial frame
# unpack parameters for the dumbbell
J = dum.J
rho1 = dum.zeta1
rho2 = dum.zeta2
# position of each mass in the asteroid frame
z1 = Ra.T.dot(pos + R.dot(rho1))
z2 = Ra.T.dot(pos + R.dot(rho2))
z = Ra.T.dot(pos) # position of COM in asteroid frame
# compute the potential at this state
true_ast.polyhedron_potential(z1)
U1 = true_ast.get_potential()
U1_grad = true_ast.get_acceleration()
true_ast.polyhedron_potential(z2)
U2 = true_ast.get_potential()
U2_grad = true_ast.get_acceleration()
F1 = dum.m1 * Ra.dot(U1_grad)
F2 = dum.m2 * Ra.dot(U2_grad)
M1 = dum.m1 * attitude.hat_map(rho1).dot(R.T.dot(Ra).dot(U1_grad))
M2 = dum.m2 * attitude.hat_map(rho2).dot(R.T.dot(Ra).dot(U2_grad))
# compute the external force and moment using the asteroid estimate
est_ast.polyhedron_potential(z1)
U1_est = est_ast.get_potential()
U1_grad_est = est_ast.get_acceleration()
est_ast.polyhedron_potential(z2)
U2_est = est_ast.get_potential()
U2_grad_est = est_ast.get_acceleration()
F1_est = dum.m1 * Ra.dot(U1_grad_est)
F2_est = dum.m2 * Ra.dot(U2_grad_est)
M1_est = dum.m1 * attitude.hat_map(rho1).dot(R.T.dot(Ra).dot(U1_grad_est))
M2_est = dum.m2 * attitude.hat_map(rho2).dot(R.T.dot(Ra).dot(U2_grad_est))
# compute the control input
u_m = controller.attitude_land_controller(t, state, M1_est + M2_est, dum,
est_ast)
u_f = controller.translation_land_controller(t, state, F1_est + F2_est,
dum, est_ast,
desired_asteroid_pos, t0,
initial_position)
pos_dot = vel
vel_dot = 1 / (dum.m1 + dum.m2) * (F1 + F2 + u_f)
R_dot = R.dot(attitude.hat_map(ang_vel)).reshape(9)
ang_vel_dot = np.linalg.inv(J).dot(-np.cross(ang_vel, J.dot(ang_vel)) +
M1 + M2 + u_m)
statedot = np.hstack((pos_dot, vel_dot, R_dot, ang_vel_dot))
return statedot
def test_eoms_inertial_R_dot(self):
np.testing.assert_allclose(
self.statedot[6:15],
R.reshape(3, 3).dot(attitude.hat_map(ang_vel)).reshape(9))
def test_hat_map_skew_symmetric(self):
np.testing.assert_allclose(
attitude.hat_map(self.x).T, -attitude.hat_map(self.x))
tf = 1e3
num_steps = 1e5
time = np.linspace(0, tf, num_steps)
periodic_pos = np.array(
[1.495746722510590, 0.000001002669660, 0.006129720493607])
periodic_vel = np.array(
[0.000000302161724, -0.000899607989820, -0.000000013286327])
ast = asteroid.Asteroid(ast_name, num_faces)
dum = dumbbell.Dumbbell(m1=500, m2=500, l=0.003)
print("Running inertial EOMS")
# set initial state for inertial EOMs
initial_pos = periodic_pos # km for center of mass in body frame
initial_vel = periodic_vel + attitude.hat_map(
ast.omega * np.array([0, 0, 1])).dot(initial_pos)
initial_R = attitude.rot2(np.pi / 2).reshape(
9) # transforms from dumbbell body frame to the inertial frame
initial_w = np.array([0.01, 0.01, 0.01])
initial_state = np.hstack((initial_pos, initial_vel, initial_R, initial_w))
def run_inertial():
i_state = integrate.odeint(dum.eoms_inertial,
initial_state,
time,
args=(ast, ),
atol=AbsTol,
rtol=RelTol)
# (i_time, i_state) = idriver.eom_inertial_driver(initial_state, time, ast, dum, AbsTol=1e-9, RelTol=1e-9)
return i_state
def test_hat_map_cross_product(self):
np.testing.assert_allclose(
attitude.hat_map(self.x).dot(self.y), np.cross(self.x, self.y))
np.testing.assert_allclose(
attitude.hat_map(self.x).dot(self.y),
-attitude.hat_map(self.y).dot(self.x))
def test_desired_attitude_satifies_kinematics(self):
np.testing.assert_array_almost_equal(
self.Rd_dot, self.Rd.dot(attitude.hat_map(self.ang_vel_d)))
def blender_asteroid_frame_sim(gen_images=False):
# simulation parameters
output_path = './visualization/blender'
asteroid_name = 'itokawa_low'
# create a HDF5 dataset
hdf5_path = './data/asteroid_circumnavigate/{}_asteroid_circumnavigate.hdf5'.format(
datetime.datetime.now().strftime("%Y-%m-%dT%H:%M:%S"))
dataset_name = 'landing'
render = 'BLENDER'
image_modulus = 1
RelTol = 1e-6
AbsTol = 1e-6
ast_name = 'itokawa'
num_faces = 64
t0 = 0
dt = 1
tf = 3600 * 1
num_steps = 3600 * 1
ast = asteroid.Asteroid(ast_name,num_faces)
dum = dumbbell.Dumbbell(m1=500, m2=500, l=0.003)
# instantiate the blender scene once
camera_obj, camera, lamp_obj, lamp, itokawa_obj, scene = blender.blender_init(render_engine=render, asteroid_name=asteroid_name)
# get some of the camera parameters
K = blender_camera.get_calibration_matrix_K_from_blender(camera)
periodic_pos = np.array([1.495746722510590,0.000001002669660,0.006129720493607])
periodic_vel = np.array([0.000000302161724,-0.000899607989820,-0.000000013286327])
# set initial state for inertial EOMs
# initial_pos = np.array([2.550, 0, 0]) # km for center of mass in body frame
initial_pos = np.array([3, 0, 0])
initial_vel = periodic_vel + attitude.hat_map(ast.omega*np.array([0,0,1])).dot(initial_pos)
initial_R = attitude.rot3(np.pi/2).reshape(9) # transforms from dumbbell body frame to the inertial frame
initial_w = np.array([0.01, 0.01, 0.01])
initial_state = np.hstack((initial_pos, initial_vel, initial_R, initial_w))
# instantiate ode object
# system = integrate.ode(eoms_controlled_blender)
system = integrate.ode(eoms.eoms_controlled_relative_blender_ode)
system.set_integrator('lsoda', atol=AbsTol, rtol=RelTol, nsteps=1000)
system.set_initial_value(initial_state, t0)
system.set_f_params(dum, ast)
i_state = np.zeros((num_steps+1, 18))
time = np.zeros(num_steps+1)
i_state[0, :] = initial_state
with h5py.File(hdf5_path) as image_data:
# create a dataset
if gen_images:
images = image_data.create_dataset(dataset_name, (244, 537, 3,
num_steps/image_modulus),
dtype='uint8')
RT_blender = image_data.create_dataset('RT',
(num_steps/image_modulus,
12))
R_i2bcam = image_data.create_dataset('R_i2bcam',
(num_steps/image_modulus, 9))
ii = 1
while system.successful() and system.t < tf:
# integrate the system and save state to an array
time[ii] = (system.t + dt)
i_state[ii, :] = (system.integrate(system.t + dt))
# generate the view of the asteroid at this state
if int(time[ii]) % image_modulus== 0 and gen_images:
img, RT, R = blender.gen_image_fixed_ast(i_state[ii,0:3],
i_state[ii,6:15].reshape((3,3)),
ast.omega * (time[ii] - 3600),
camera_obj, camera,
lamp_obj, lamp,
itokawa_obj, scene,
[5, 0, 1], 'test')
images[:, :, :, ii//image_modulus - 1] = img
RT_blender[ii//image_modulus -1, :] = RT.reshape(12)
R_i2bcam[ii//image_modulus -1, :] = R.reshape(9)
# do some image processing and visual odometry
print(system.t)
ii += 1
image_data.create_dataset('K', data=K)
image_data.create_dataset('i_state', data=i_state)
image_data.create_dataset('time', data=time)
def eoms_controlled_relative_blender_ode(t, state, dum, ast):
"""Relative EOMS defined in the rotating asteroid frame
This function defines the motion of a dumbbell spacecraft in orbit around
an asteroid.
The EOMS are defined relative to the asteroid itself, which is in a state
of constant rotation.
You need to use this function with scipy.integrate.ode class
This is setup to test Blender using a fixed asteroid
Inputs:
t - current time of simulation (sec)
state - (18,) relative state of dumbbell with respect to asteroid
pos - state[0:3] in km position of the dumbbell with respect to the
asteroid and defined in the asteroid fixed frame
vel - state[3:6] in km/sec is the velocity of dumbbell wrt the
asteroid and defined in the asteroid fixed frame
R - state[6:15] rotation matrix which converts vectors from the
dumbbell frame to the asteroid frame
w - state[15:18] rad/sec angular velocity of the dumbbell wrt
inertial frame and defined in the asteroid frame
ast - asteroid object
Output:
state_dot - (18,) derivative of state. The order is the same as the input state.
"""
# unpack the state
pos = state[0:3] # location of the COM of dumbbell in asteroid fixed frame
vel = state[
3:
6] # vel of com wrt to asteroid expressed in the asteroid fixed frame
R = np.reshape(
state[6:15],
(3, 3)) # sc body frame to asteroid body frame R = R_A^T R_1
w = state[
15:
18] # angular velocity of sc wrt inertial frame and expressed in asteroid fixed frame
Ra = attitude.rot3(ast.omega * t,
'c') # asteroid body frame to inertial frame
# unpack parameters for the dumbbell
m1 = dum.m1
m2 = dum.m2
m = m1 + m2
J = dum.J
Jr = R.dot(J).dot(R.T)
Wa = ast.omega * np.array([0, 0, 1]) # angular velocity vector of asteroid
# the position of each mass in the dumbbell body frame
rho1 = dum.zeta1
rho2 = dum.zeta2
# position of each mass in the asteroid frame
z1 = pos + R.dot(rho1)
z2 = pos + R.dot(rho2)
z = pos # position of COM in asteroid frame
# compute the potential at this state
(U1, U1_grad, U1_grad_mat, U1laplace) = ast.polyhedron_potential(z1)
(U2, U2_grad, U2_grad_mat, U2laplace) = ast.polyhedron_potential(z2)
# force due to each mass expressed in asteroid body frame
F1 = m1 * U1_grad
F2 = m2 * U2_grad
M1 = m1 * attitude.hat_map(R.dot(rho1)).dot(U1_grad)
M2 = m2 * attitude.hat_map(R.dot(rho2)).dot(U2_grad)
des_tran_tuple = controller.asteroid_circumnavigate(t, 3600, 1)
des_att_tuple = controller.asteroid_pointing(t, state, ast)
u_f = controller.translation_controller_asteroid(t, state, F1 + F2, dum,
ast, des_tran_tuple)
u_m = controller.attitude_controller_asteroid(t, state, M1 + M2, dum, ast,
des_att_tuple)
# state derivatives
pos_dot = vel - attitude.hat_map(Wa).dot(pos)
vel_dot = 1 / m * (F1 + F2 - m * attitude.hat_map(Wa).dot(vel) + u_f)
# vel_dot = 1/m * (F_com)
R_dot = attitude.hat_map(w).dot(R) - attitude.hat_map(Wa).dot(R)
R_dot = R_dot.reshape(9)
w_dot = np.linalg.inv(Jr).dot(M1 + M2 -
Jr.dot(attitude.hat_map(Wa)).dot(w) + u_m)
state_dot = np.hstack((pos_dot, vel_dot, R_dot, w_dot))
return state_dot