Coverage for D:\Ralf Gerlich\git\modypy\modypy\blocks\rigid.py: 0%

# Coverage for D:\Ralf Gerlich\git\modypy\modypy\blocks\rigid.py : 0%

## 55 statements

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"""

2 Blocks for stiff body dynamics

3"""

4 import numpy as np

5 import numpy.linalg as linalg

6

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

8

9

10 class 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

127 class 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])