Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
"""
Object state defined by:
x_t: 12, | (p_t, q_t, v_t, H_t) full state vector
p_t: 3, | 3d position of center of mass [m]
q_t: 3, | exponential map representation for 3d orientation
where q/|q| is the axis
and |q| is the rotation about that axis [rad]
v_t: 3, | velocity of center of mass [m/s]
H_t: 3, | angular momentum H_I^c [kg m^2 / s]
about center of mass in body frame
"""
import numpy as np
from misc import quaternion as qua
class FreeFloating(object):
"""
Model 6dof (translation + rotation) motion imparted by thrust inputs
in absence of other forces.
x_dot = fx + B(x)
p_dot = v_t + 0
q_dot = [0.5(J_B^c)^-1 A(q)H_I^c] \dot q + 0
- where q is quaternion representation of q_t
- dot is the quaternion dot multiplication
v_dot = 0 + A(q) f/m = A(q) b_part
H_dot = 0 + \sum_{i=1}^n r_i \cross f_i u_i
Use exponential map to represent rotations, converting to
quaternions to create the rotation matrix when needed.
"""
def __init__(self, mounted_thruster_model):
self.m = 12
self.A = np.zeros((self.m, self.m))
self.A[:3, 6:9] = np.eye(3)
f = mounted_thruster_model.f
r = mounted_thruster_model.r
self.n = f.shape[1]
self.B = np.zeros((self.m, self.n))
self.b_part = f / mounted_thruster_model.m
self.B[9:, :] = np.cross(r.T, f.T).T
self.J_Bc_inv = np.linalg.inv(mounted_thruster_model.im)
def step(self, x_t, u_t):
operators = self.get_process_operators(x_t)
fx, gx = operators['fx'], operators['gx']
x_dot = fx + gx.dot(u_t)
return x_dot
def get_process_operators(self, x_t):
"""
Return operators which may be based on current state
:param x_t:
:return: dict{fx, gx, B(x_t)}
"""
B_t = self.B
q = qua.exp_map2quaternion(x_t[3:6])
Aq = qua.quaternion2rotation_matrix(q)
B_t[6:9, :] = Aq.T.dot(self.b_part)
gx = B_t
omega = self.J_Bc_inv.dot(x_t[9:])
q_quaternion = 0.5 * qua.dot_mult_matrix(q).dot(omega)
fx_q_dot = qua.quaternion2exp_map(q_quaternion)
fx = self.A.dot(x_t)
fx[3:6] = fx_q_dot
return dict(fx=fx, gx=gx, A=self.A, B=B_t)
def reset_state(self, x_t):
"""
Outside of IVP solver, reset exp map here to avoid
singularity at 2pi (= identity rotation but magnitude != 0)
:param x_t:
:return:
"""
x_t[3:6] = qua.reset_exp_map(x_t[3:6])
return x_t
def get_mn(self):
return self.m, self.n