Hide keyboard shortcuts

Hot-keys on this page

r m x p   toggle line displays

j k   next/prev highlighted chunk

0   (zero) top of page

1   (one) first highlighted chunk

1""" 

2Blocks for stiff body dynamics 

3""" 

4import numpy as np 

5import numpy.linalg as linalg 

6 

7from modypy.model import Block, Port, SignalState, signal_method 

8 

9 

10class RigidBody6DOFFlatEarth(Block): 

11 """A block representing the motion of a rigid body in 6 degrees of freedom 

12 relative to a flat earth reference frame. 

13 

14 The flat earth reference frame assumes the x-axis pointing to the north, 

15 the y-axis pointing to the east, and the z-axis pointing down. 

16 

17 The body reference frame assumes the x-axis pointing to the front, the 

18 y-axis pointing to the right, and the z-axis pointing down. 

19 

20 Both reference frames are right-handed systems. 

21 

22 The block assumes a constant mass and constant moment of inertia. 

23 

24 The block accepts the applied forces and moments in the body reference 

25 frame as input, in this order. 

26 

27 It provides as output (in this order) 

28 - the velocity in the earth reference frame, 

29 - the position in the earth reference frame, 

30 - the coordinate transformation matrix from the body to the earth 

31 reference frame, 

32 - the velocity in the body reference frame, and 

33 - the angular rates in the body reference frame. 

34 """ 

35 

36 def __init__(self, 

37 owner, 

38 mass, 

39 moment_of_inertia, 

40 initial_velocity_earth=None, 

41 initial_position_earth=None, 

42 initial_transformation=None, 

43 initial_angular_rates_earth=None): 

44 Block.__init__(self, owner) 

45 self.mass = mass 

46 self.moment_of_inertia = moment_of_inertia 

47 

48 self.forces_body = Port(shape=3) 

49 self.moments_body = Port(shape=3) 

50 

51 if initial_transformation is None: 

52 initial_transformation = np.eye(3) 

53 

54 self.velocity_earth = \ 

55 SignalState(self, 

56 shape=3, 

57 derivative_function=self.velocity_earth_dot, 

58 initial_condition=initial_velocity_earth) 

59 self.position_earth = \ 

60 SignalState(self, 

61 shape=3, 

62 derivative_function=self.position_earth_dot, 

63 initial_condition=initial_position_earth) 

64 self.omega_earth = \ 

65 SignalState(self, 

66 shape=3, 

67 derivative_function=self.omega_earth_dot, 

68 initial_condition=initial_angular_rates_earth) 

69 self.dcm = \ 

70 SignalState(self, 

71 shape=(3, 3), 

72 derivative_function=self.dcm_dot, 

73 initial_condition=initial_transformation) 

74 

75 def velocity_earth_dot(self, data): 

76 """Calculates the acceleration in the earth reference frame 

77 """ 

78 forces_earth = self.dcm(data) @ self.forces_body(data) 

79 accel_earth = forces_earth / self.mass 

80 return accel_earth 

81 

82 def position_earth_dot(self, data): 

83 """Calculates the velocity in the earth reference frame 

84 """ 

85 return self.velocity_earth(data) 

86 

87 def omega_earth_dot(self, data): 

88 """Calculate the angular acceleration in the earth reference frame 

89 """ 

90 moments_earth = self.dcm(data) @ self.moments_body(data) 

91 ang_accel_earth = linalg.solve(self.moment_of_inertia, moments_earth) 

92 return ang_accel_earth 

93 

94 def dcm_dot(self, data): 

95 """Calculate the derivative of the direct cosine matrix 

96 """ 

97 omega_earth = self.omega_earth(data) 

98 skew_sym_matrix = np.array([ 

99 [0, -omega_earth[2], omega_earth[1]], 

100 [omega_earth[2], 0, -omega_earth[0]], 

101 [-omega_earth[1], omega_earth[0], 0] 

102 ]) 

103 return skew_sym_matrix @ self.dcm(data) 

104 

105 @signal_method(shape=(3, 3)) 

106 def dcm_inverse(self, data): 

107 """The inverse of the direct cosine matrix""" 

108 return np.swapaxes(self.dcm(data), 0, 1) 

109 

110 @signal_method(shape=3) 

111 def velocity_body(self, data): 

112 """Calculate the velocity in the body reference frame""" 

113 

114 return np.einsum("ij...,j...->i...", 

115 self.dcm_inverse(data), 

116 self.velocity_earth(data)) 

117 

118 @signal_method(shape=3) 

119 def omega_body(self, data): 

120 """Calculate the angular velocity in the body reference frame""" 

121 

122 return np.einsum("ij...,j...->i...", 

123 self.dcm_inverse(data), 

124 self.omega_earth(data)) 

125 

126 

127class DirectCosineToEuler(Block): 

128 """A block translating a direct cosine matrix to Euler angles. 

129 

130 This block has a 3x3 direct cosine matrix (DCM) as input and 

131 provides the euler angles (psi, theta, phi, in radians) for yaw, 

132 pitch and roll as output. 

133 """ 

134 

135 # yaw: psi 

136 # pitch: theta 

137 # roll: phi 

138 def __init__(self, parent): 

139 Block.__init__(self, parent) 

140 

141 self.dcm = Port(shape=(3, 3)) 

142 

143 @signal_method 

144 def yaw(self, data): 

145 """Calculate the yaw angle for the given direct cosine matrix 

146 """ 

147 dcm = self.dcm(data) 

148 return np.arctan2(dcm[1, 0], dcm[0, 0]) 

149 

150 @signal_method 

151 def pitch(self, data): 

152 """Calculate the pitch angle for the given direct cosine matrix 

153 """ 

154 dcm = self.dcm(data) 

155 return np.arctan2(-dcm[2, 0], np.sqrt(dcm[0, 0] ** 2 + dcm[1, 0] ** 2)) 

156 

157 @signal_method 

158 def roll(self, data): 

159 """Calculate the roll angle for the given direct cosine matrix 

160 """ 

161 dcm = self.dcm(data) 

162 return np.arctan2(dcm[2, 1], dcm[2, 2])