Astrobee: A Holonomic Free-Flying Space Robot¶
Note
You can download this example as a Python script:
astrobee.py or Jupyter notebook:
astrobee.ipynb.
Astrobee is a new generation of free-flying robots aboard the International Space Station (ISS). It is a cubic robot with sides measuring about 30 cm each. The robot is propelled by two fans located on the sides of the robot and servo-actuated louvred vent nozzles, which allow for full six degree-of-freedom holonomic control [Smith2016]. Here, the nonlinear dynamics of Astrobee are modeled using Kane’s method and the holonomic behavior of the system is demonstrated. After derivation of the nonlinear equations of motion, the system is linearized about a chosen operating point to obtain an explicit first order state-space representation, which can be used for control design.
import sympy as sm
import sympy.physics.mechanics as me
from pydy.system import System
import numpy as np
import matplotlib.pyplot as plt
from pydy.codegen.ode_function_generators import generate_ode_function
from scipy.integrate import odeint
import scipy.io as sio
me.init_vprinting()
Reference Frames¶
ISS = me.ReferenceFrame('N') # ISS RF
B = me.ReferenceFrame('B') # body RF
q1, q2, q3 = me.dynamicsymbols('q1:4') # attitude coordinates (Euler angles)
B.orient(ISS, 'Body', (q1, q2, q3), 'xyz') # body RF
t = me.dynamicsymbols._t
Significant Points¶
O = me.Point('O') # fixed point in the ISS
O.set_vel(ISS, 0)
x, y, z = me.dynamicsymbols('x, y, z') # translation coordinates (position of the mass-center of Astrobee relative to 'O')
l = sm.symbols('l') # length of Astrobee (side of cube)
C = O.locatenew('C', x * ISS.x + y * ISS.y + z * ISS.z) # Astrobee CM
Kinematical Differential Equations¶
ux = me.dynamicsymbols('u_x')
uy = me.dynamicsymbols('u_y')
uz = me.dynamicsymbols('u_z')
u1 = me.dynamicsymbols('u_1')
u2 = me.dynamicsymbols('u_2')
u3 = me.dynamicsymbols('u_3')
z1 = sm.Eq(ux, x.diff())
z2 = sm.Eq(uy, y.diff())
z3 = sm.Eq(uz, z.diff())
z4 = sm.Eq(u1, q1.diff())
z5 = sm.Eq(u2, q2.diff())
z6 = sm.Eq(u3, q3.diff())
u = sm.solve([z1, z2, z3, z4, z5, z6], x.diff(), y.diff(), z.diff(), q1.diff(), q2.diff(), q3.diff())
u
\[\displaystyle \left\{ \dot{q}_{1} : u_{1}, \ \dot{q}_{2} : u_{2}, \ \dot{q}_{3} : u_{3}, \ \dot{x} : u_{x}, \ \dot{y} : u_{y}, \ \dot{z} : u_{z}\right\}\]
Translational Motion¶
Velocity¶
C.set_vel(ISS, C.pos_from(O).dt(ISS).subs(u))
V_B_ISS_ISS = C.vel(ISS)
V_B_ISS_ISS # "velocity of Astrobee CM w.r.t ISS RF expressed in ISS RF"
\[\displaystyle u_{x}\mathbf{\hat{n}_x} + u_{y}\mathbf{\hat{n}_y} + u_{z}\mathbf{\hat{n}_z}\]
Acceleration¶
A_B_ISS_ISS = C.acc(ISS).subs(u) #.subs(ud)
A_B_ISS_ISS # "acceleration of Astrobee CM w.r.t ISS RF expressed in ISS RF"
\[\displaystyle \dot{u}_{x}\mathbf{\hat{n}_x} + \dot{u}_{y}\mathbf{\hat{n}_y} + \dot{u}_{z}\mathbf{\hat{n}_z}\]
Angular Motion¶
Angular Velocity¶
B.set_ang_vel(ISS, B.ang_vel_in(ISS).subs(u))
Omega_B_ISS_B = B.ang_vel_in(ISS)
Omega_B_ISS_B # "angular velocity of body RF w.r.t ISS RF expressed in body RF"
\[\displaystyle (\frac{u_{1} \cos{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} \sin{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}})\mathbf{\hat{b}_x} + (- \frac{u_{1} \sin{\left(q_{3} \right)} \cos{\left(q_{2} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}})\mathbf{\hat{b}_y} + (u_{1} \sin{\left(q_{2} \right)} + u_{3})\mathbf{\hat{b}_z}\]
Angular Acceleration¶
Alpha_B_ISS_B = B.ang_acc_in(ISS).subs(u) #.subs(ud)
Alpha_B_ISS_B # "angular acceleration of body RF w.r.t ISS RF expressed in body RF"
\[\displaystyle (- \frac{u_{1} u_{2} \sin{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} - \frac{u_{1} u_{3} \sin{\left(q_{3} \right)} \cos{\left(q_{2} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} u_{3} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{\sin{\left(q_{3} \right)} \dot{u}_{2}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{\cos{\left(q_{2} \right)} \cos{\left(q_{3} \right)} \dot{u}_{1}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}})\mathbf{\hat{b}_x} + (\frac{u_{1} u_{2} \sin{\left(q_{2} \right)} \sin{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} - \frac{u_{1} u_{3} \cos{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} - \frac{u_{2} u_{3} \sin{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} - \frac{\sin{\left(q_{3} \right)} \cos{\left(q_{2} \right)} \dot{u}_{1}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{\cos{\left(q_{3} \right)} \dot{u}_{2}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}})\mathbf{\hat{b}_y} + (u_{1} u_{2} \cos{\left(q_{2} \right)} + \sin{\left(q_{2} \right)} \dot{u}_{1} + \dot{u}_{3})\mathbf{\hat{b}_z}\]
Mass and Inertia¶
m = sm.symbols('m') # Astrobee mass
Ix, Iy, Iz = sm.symbols('I_x, I_y, I_z') # principal moments of inertia
I = me.inertia(B, Ix, Iy, Iz) # inertia dyadic
I
\[\displaystyle I_{x}\mathbf{\hat{b}_x}\otimes \mathbf{\hat{b}_x} + I_{y}\mathbf{\hat{b}_y}\otimes \mathbf{\hat{b}_y} + I_{z}\mathbf{\hat{b}_z}\otimes \mathbf{\hat{b}_z}\]
Loads¶
Forces¶
Fx_mag, Fy_mag, Fz_mag = me.dynamicsymbols('Fmag_x, Fmag_y, Fmag_z')
Fx = Fx_mag * ISS.x
Fy = Fy_mag * ISS.y
Fz = Fz_mag * ISS.z
Fx, Fy, Fz
\[\displaystyle \left( \left|{F}\right|_{x}\mathbf{\hat{n}_x}, \ \left|{F}\right|_{y}\mathbf{\hat{n}_y}, \ \left|{F}\right|_{z}\mathbf{\hat{n}_z}\right)\]
Torques¶
T1_mag, T2_mag, T3_mag = me.dynamicsymbols('Tmag_1, Tmag_2, Tmag_3')
T1 = T1_mag * B.x
T2 = T2_mag * B.y
T3 = T3_mag * B.z
T1, T2, T3
\[\displaystyle \left( \left|{T}\right|_{1}\mathbf{\hat{b}_x}, \ \left|{T}\right|_{2}\mathbf{\hat{b}_y}, \ \left|{T}\right|_{3}\mathbf{\hat{b}_z}\right)\]
Kane’s Method¶
kdes = [z1.rhs - z1.lhs,
z2.rhs - z2.lhs,
z3.rhs - z3.lhs,
z4.rhs - z4.lhs,
z5.rhs - z5.lhs,
z6.rhs - z6.lhs]
body = me.RigidBody('body', C, B, m, (I, C))
bodies = [body]
loads = [
(C, Fx),
(C, Fy),
(C, Fz),
(B, T1),
(B, T2),
(B, T3)
]
kane = me.KanesMethod(ISS, (x, y, z, q1, q2, q3), (ux, uy, uz, u1, u2, u3), kd_eqs=kdes)
fr, frstar = kane.kanes_equations(bodies, loads=loads)
Simulation¶
sys = System(kane)
sys.constants_symbols
sys.constants = {
Ix: 0.1083,
Iy: 0.1083,
Iz: 0.1083,
m: 7
}
sys.constants
\[\displaystyle \left\{ I_{x} : 0.1083, \ I_{y} : 0.1083, \ I_{z} : 0.1083, \ m : 7\right\}\]
sys.times = np.linspace(0.0, 50.0, num=1000)
sys.coordinates
\[\displaystyle \left[ x, \ y, \ z, \ q_{1}, \ q_{2}, \ q_{3}\right]\]
sys.speeds
\[\displaystyle \left[ u_{x}, \ u_{y}, \ u_{z}, \ u_{1}, \ u_{2}, \ u_{3}\right]\]
sys.states
\[\displaystyle \left[ x, \ y, \ z, \ q_{1}, \ q_{2}, \ q_{3}, \ u_{x}, \ u_{y}, \ u_{z}, \ u_{1}, \ u_{2}, \ u_{3}\right]\]
sys.initial_conditions = {
x: 0.0,
y: 0.0,
z: 0.0,
q1: 0.0,
q2: 0.0,
q3: 0.0,
ux: 0.2,
uy: 0.0,
uz: 0.0,
u1: 0.0,
u2: 0.0,
u3: 0.5
}
sys.specifieds_symbols
\[\displaystyle \left\{\left|{F}\right|_{x}, \left|{F}\right|_{y}, \left|{F}\right|_{z}, \left|{T}\right|_{1}, \left|{T}\right|_{2}, \left|{T}\right|_{3}\right\}\]
sys.specifieds = {
Fx_mag: 0.0,
Fy_mag: 0.0,
Fz_mag: 0.0,
T1_mag: 0.0,
T2_mag: 0.0,
T3_mag: 0.0
}
states = sys.integrate()
import matplotlib.pyplot as plt
fig, ax = plt.subplots()
ax.plot(sys.times, states)
ax.set_xlabel('{} [s]'.format(sm.latex(t, mode='inline')));
ax.set_ylabel('States');
ax.legend(['$x$', '$y$', '$z$', '$q_1$', '$q_2$', '$q_3$', '$u_x$', '$u_y$', '$u_z$', '$u_1$', '$u_2$', '$u_3$'], fontsize=10)
plt.show()
3D Visualization¶
from pydy.viz import Box, Cube, Sphere, Cylinder, VisualizationFrame, Scene
l = 0.32
body_m_shape = Box(l, (1/2) * l, (2/3) * l, color='black', name='body_m_shape')
body_l_shape = Box(l, (1/4) * l, l, color='green', name='body_l_shape')
body_r_shape = Box(l, (1/4) * l, l, color='green', name='body_r_shape')
v1 = VisualizationFrame('Body_m',
B,
C.locatenew('C_m', (1/6) * l * B.z),
body_m_shape)
v2 = VisualizationFrame('Body_l',
B,
C.locatenew('C_l', (3/8) * l * -B.y),
body_l_shape)
v3 = VisualizationFrame('Body_r',
B,
C.locatenew('C_r', (3/8) * l * B.y),
body_r_shape)
scene = Scene(ISS, O, system=sys)
scene.visualization_frames = [v1, v2, v3]
scene.display_jupyter(axes_arrow_length=1.0)
Linearization¶
f = fr + frstar
f
\[\begin{split}\displaystyle \left[\begin{matrix}- m \dot{u}_{x} + \left|{F}\right|_{x}\\- m \dot{u}_{y} + \left|{F}\right|_{y}\\- m \dot{u}_{z} + \left|{F}\right|_{z}\\- I_{z} \sin{\left(q_{2} \right)} \dot{u}_{3} - \left(\frac{I_{x} \sin{\left(q_{3} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\left(\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}\right)^{2}} - \frac{I_{y} \sin{\left(q_{3} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\left(\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}\right)^{2}}\right) \dot{u}_{2} - \left(- I_{x} \left(- \frac{u_{1} \sin{\left(q_{3} \right)} \cos{\left(q_{2} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right) \left(\frac{u_{1} \cos{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} \sin{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right) + I_{y} \left(- \frac{u_{1} \sin{\left(q_{3} \right)} \cos{\left(q_{2} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right) \left(\frac{u_{1} \cos{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} \sin{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right) + I_{z} u_{1} u_{2} \cos{\left(q_{2} \right)}\right) \sin{\left(q_{2} \right)} - \left(\frac{I_{x} \cos^{2}{\left(q_{2} \right)} \cos^{2}{\left(q_{3} \right)}}{\left(\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}\right)^{2}} + \frac{I_{y} \sin^{2}{\left(q_{3} \right)} \cos^{2}{\left(q_{2} \right)}}{\left(\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}\right)^{2}} + I_{z} \sin^{2}{\left(q_{2} \right)}\right) \dot{u}_{1} + \left|{T}\right|_{3} \sin{\left(q_{2} \right)} - \frac{\left(I_{x} \left(- \frac{u_{1} u_{2} \sin{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} - \frac{u_{1} u_{3} \sin{\left(q_{3} \right)} \cos{\left(q_{2} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} u_{3} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right) - I_{y} \left(u_{1} \sin{\left(q_{2} \right)} + u_{3}\right) \left(- \frac{u_{1} \sin{\left(q_{3} \right)} \cos{\left(q_{2} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right) + I_{z} \left(u_{1} \sin{\left(q_{2} \right)} + u_{3}\right) \left(- \frac{u_{1} \sin{\left(q_{3} \right)} \cos{\left(q_{2} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right)\right) \cos{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{\left(I_{x} \left(u_{1} \sin{\left(q_{2} \right)} + u_{3}\right) \left(\frac{u_{1} \cos{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} \sin{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right) + I_{y} \left(\frac{u_{1} u_{2} \sin{\left(q_{2} \right)} \sin{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} - \frac{u_{1} u_{3} \cos{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} - \frac{u_{2} u_{3} \sin{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right) - I_{z} \left(u_{1} \sin{\left(q_{2} \right)} + u_{3}\right) \left(\frac{u_{1} \cos{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} \sin{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right)\right) \sin{\left(q_{3} \right)} \cos{\left(q_{2} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{\left|{T}\right|_{1} \cos{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} - \frac{\left|{T}\right|_{2} \sin{\left(q_{3} \right)} \cos{\left(q_{2} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\\- \left(\frac{I_{x} \sin^{2}{\left(q_{3} \right)}}{\left(\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}\right)^{2}} + \frac{I_{y} \cos^{2}{\left(q_{3} \right)}}{\left(\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}\right)^{2}}\right) \dot{u}_{2} - \left(\frac{I_{x} \sin{\left(q_{3} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\left(\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}\right)^{2}} - \frac{I_{y} \sin{\left(q_{3} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\left(\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}\right)^{2}}\right) \dot{u}_{1} - \frac{\left(I_{x} \left(- \frac{u_{1} u_{2} \sin{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} - \frac{u_{1} u_{3} \sin{\left(q_{3} \right)} \cos{\left(q_{2} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} u_{3} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right) - I_{y} \left(u_{1} \sin{\left(q_{2} \right)} + u_{3}\right) \left(- \frac{u_{1} \sin{\left(q_{3} \right)} \cos{\left(q_{2} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right) + I_{z} \left(u_{1} \sin{\left(q_{2} \right)} + u_{3}\right) \left(- \frac{u_{1} \sin{\left(q_{3} \right)} \cos{\left(q_{2} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right)\right) \sin{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} - \frac{\left(I_{x} \left(u_{1} \sin{\left(q_{2} \right)} + u_{3}\right) \left(\frac{u_{1} \cos{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} \sin{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right) + I_{y} \left(\frac{u_{1} u_{2} \sin{\left(q_{2} \right)} \sin{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} - \frac{u_{1} u_{3} \cos{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} - \frac{u_{2} u_{3} \sin{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right) - I_{z} \left(u_{1} \sin{\left(q_{2} \right)} + u_{3}\right) \left(\frac{u_{1} \cos{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} \sin{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right)\right) \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{\left|{T}\right|_{1} \sin{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{\left|{T}\right|_{2} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\\I_{x} \left(- \frac{u_{1} \sin{\left(q_{3} \right)} \cos{\left(q_{2} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right) \left(\frac{u_{1} \cos{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} \sin{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right) - I_{y} \left(- \frac{u_{1} \sin{\left(q_{3} \right)} \cos{\left(q_{2} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right) \left(\frac{u_{1} \cos{\left(q_{2} \right)} \cos{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}} + \frac{u_{2} \sin{\left(q_{3} \right)}}{\sin^{2}{\left(q_{3} \right)} + \cos^{2}{\left(q_{3} \right)}}\right) - I_{z} u_{1} u_{2} \cos{\left(q_{2} \right)} - I_{z} \sin{\left(q_{2} \right)} \dot{u}_{1} - I_{z} \dot{u}_{3} + \left|{T}\right|_{3}\end{matrix}\right]\end{split}\]
V = {
x: 0.0,
y: 0.0,
z: 0.0,
q1: 0.0,
q2: 0.0,
q3: 0.0,
ux: 0.0,
uy: 0.0,
uz: 0.0,
u1: 0.0,
u2: 0.0,
u3: 0.0,
Fx_mag: 0.0,
Fy_mag: 0.0,
Fz_mag: 0.0,
T1_mag: 0.0,
T2_mag: 0.0,
T3_mag: 0.0
}
V_keys = sm.Matrix([ v for v in V.keys() ])
V_values = sm.Matrix([ v for v in V.values() ])
us = sm.Matrix([ux, uy, uz, u1, u2, u3])
us_diff = sm.Matrix([ux.diff(), uy.diff(), uz.diff(), u1.diff(), u2.diff(), u3.diff()])
qs = sm.Matrix([x, y, z, q1, q2, q3])
rs = sm.Matrix([Fx_mag, Fy_mag, Fz_mag, T1_mag, T2_mag, T3_mag])
Ml = f.jacobian(us_diff).subs(sys.constants).subs(V)
Ml
\[\begin{split}\displaystyle \left[\begin{matrix}-7 & 0 & 0 & 0 & 0 & 0\\0 & -7 & 0 & 0 & 0 & 0\\0 & 0 & -7 & 0 & 0 & 0\\0 & 0 & 0 & -0.1083 & 0 & 0\\0 & 0 & 0 & 0 & -0.1083 & 0\\0 & 0 & 0 & 0 & 0 & -0.1083\end{matrix}\right]\end{split}\]
Cl = f.jacobian(us).subs(V)
Cl.subs(sys.constants)
\[\begin{split}\displaystyle \left[\begin{matrix}0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0\end{matrix}\right]\end{split}\]
Kl = f.jacobian(qs).subs(V)
sm.simplify(Kl.subs(sys.constants))
\[\begin{split}\displaystyle \left[\begin{matrix}0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0\end{matrix}\right]\end{split}\]
Hl = -f.jacobian(rs).subs(V)
sm.simplify(Hl.subs(sys.constants))
\[\begin{split}\displaystyle \left[\begin{matrix}-1 & 0 & 0 & 0 & 0 & 0\\0 & -1 & 0 & 0 & 0 & 0\\0 & 0 & -1 & 0 & 0 & 0\\0 & 0 & 0 & -1 & 0 & 0\\0 & 0 & 0 & 0 & -1 & 0\\0 & 0 & 0 & 0 & 0 & -1\end{matrix}\right]\end{split}\]
A = sm.Matrix([[(-Ml.inv()*Cl), (-Ml.inv()*Kl)], [(sm.eye(6)), sm.zeros(6, 6)]])
sm.simplify(A.subs(sys.constants))
\[\begin{split}\displaystyle \left[\begin{array}{cccccccccccc}0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0\end{array}\right]\end{split}\]
B = sm.Matrix([[Ml.inv() * Hl], [sm.zeros(6, 6)]])
sm.nsimplify(B.subs(sys.constants))
\[\begin{split}\displaystyle \left[\begin{matrix}\frac{1}{7} & 0 & 0 & 0 & 0 & 0\\0 & \frac{1}{7} & 0 & 0 & 0 & 0\\0 & 0 & \frac{1}{7} & 0 & 0 & 0\\0 & 0 & 0 & 9.23361034164358 & 0 & 0\\0 & 0 & 0 & 0 & 9.23361034164358 & 0\\0 & 0 & 0 & 0 & 0 & 9.23361034164358\\0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0\end{matrix}\right]\end{split}\]
References¶
- Smith2016
Smith, T., Barlow, J., Bualat, M., Fong, T., Provencher, C., Sanchez, H., & Smith, E. (2016). Astrobee: A new platform for free-flying robotics on the international space station.