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

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
7from modypy.model import Block, Port, SignalState, signal_method
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.
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.
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.
20 Both reference frames are right-handed systems.
22 The block assumes a constant mass and constant moment of inertia.
24 The block accepts the applied forces and moments in the body reference
25 frame as input, in this order.
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 """
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
48 self.forces_body = Port(shape=3)
49 self.moments_body = Port(shape=3)
51 if initial_transformation is None:
52 initial_transformation = np.eye(3)
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)
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
82 def position_earth_dot(self, data):
83 """Calculates the velocity in the earth reference frame
84 """
85 return self.velocity_earth(data)
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
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)
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)
110 @signal_method(shape=3)
111 def velocity_body(self, data):
112 """Calculate the velocity in the body reference frame"""
114 return np.einsum("ij...,j...->i...",
115 self.dcm_inverse(data),
116 self.velocity_earth(data))
118 @signal_method(shape=3)
119 def omega_body(self, data):
120 """Calculate the angular velocity in the body reference frame"""
122 return np.einsum("ij...,j...->i...",
123 self.dcm_inverse(data),
124 self.omega_earth(data))
127class DirectCosineToEuler(Block):
128 """A block translating a direct cosine matrix to Euler angles.
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 """
135 # yaw: psi
136 # pitch: theta
137 # roll: phi
138 def __init__(self, parent):
139 Block.__init__(self, parent)
141 self.dcm = Port(shape=(3, 3))
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])
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))
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])