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 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 test_hat_vee_map_inverse(self):
np.testing.assert_allclose(attitude.vee_map(attitude.hat_map(self.x)),
self.x)
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
# rotate the body vector to the inertial frame
R_b2i = attitude.rot2(lat[jj]).T.dot(attitude.rot3(lon[ii]))
sen_inertial = R_b2i.dot(sen)
sen_array[:, ii*den + 0] = sen_inertial
psi_attract = 1/2 * np.trace(G.dot(np.eye(3, 3) - R_des.T.dot(R_b2i)))
if np.dot(sen_inertial, con) < con_angle:
psi_avoid = -1 / alpha * np.log(- (np.dot(sen_inertial, con) - con_angle) / (1 + con_angle)) + 1
else:
psi_avoid = 11
psi_total = psi_attract * psi_avoid
er = 1/2 * attitude.vee_map(G.dot(R_des.T).dot(R_b2i) - R_b2i.T.dot(R_des).dot(G))
er_attract_array[:, ii*den + jj] = er.T
psi_avoid_array[ii, jj] = psi_avoid
psi_attract_array[ii, jj] = psi_attract
psi_total_array[ii, jj] = psi_total
g = 1 + -1/alpha * np.log(- (angle - con_angle) / (1+con_angle))
eRB = np.absolute(1 / alpha * np.sin(np.arccos(angle)) / (angle - con_angle))
# make sure nothing is plotted beyond the range of our plot
psi_avoid_array[psi_avoid_array > 3] = 3
psi_attract_array[psi_attract_array > 3] = 3
psi_total_array[psi_total_array > 3] = 3
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
