Coverage for src/gncpy/dynamics/basic/clohessy_wiltshire_orbit.py: 76%

1import numpy as np 

2from warnings import warn 

3 

4import gncpy.dynamics._dynamics as cpp_bindings 

5import gncpy.control._control as cpp_control 

6from .clohessy_wiltshire_orbit2d import ClohessyWiltshireOrbit2d 

7 

8 

9class ClohessyWiltshireOrbit(ClohessyWiltshireOrbit2d): 

10 """Implements the Clohessy Wiltshire orbit model. 

11 

12 This adds on the z component to make the model 3d. 

13 

14 Attributes 

15 ---------- 

16 mean_motion :float 

17 mean motion of reference spacecraft 

18 """ 

19 

20 state_names = ("x pos", "y pos", "z pos", "x vel", "y vel", "z vel") 

21 

22 def __init__(self, **kwargs): 

23 super().__init__(**kwargs) 

24 self.__controlParams = cpp_control.ControlParams() 

25 self.__stateTransParams = cpp_bindings.StateTransParams() 

26 self.__model = cpp_bindings.ClohessyWiltshire(0.1, 0.01) 

27 if self.mean_motion is not None: 

28 self.__model = cpp_bindings.ClohessyWiltshire(0.01, self.mean_motion) 

29 else: 

30 self.__model = cpp_bindings.ClohessyWiltshire(0.01, 0.0) 

31 if "control_model" in kwargs and kwargs["control_model"] is not None: 

32 self.__model.set_control_model(kwargs("control_model")) 

33 

34 @property 

35 def allow_cpp(self): 

36 return True 

37 

38 @property 

39 def control_model(self): 

40 warn("viewing the control model is not supported for this class") 

41 return None 

42 

43 @control_model.setter 

44 def control_model(self, model): 

45 if isinstance(model, cpp_control.ILinearControlModel): 

46 self._control_model = model 

47 self.__model.set_control_model(self._control_model) 

48 else: 

49 raise TypeError("must be ILinearControlModel type") 

50 

51 def get_input_mat(self, timestep, *ctrl_args): 

52 """Calculates the input matrix from the control model. 

53 

54 This calculates the jacobian of the control model. If no control model 

55 is specified than it returns a zero matrix. 

56 

57 Parameters 

58 ---------- 

59 timestep : float 

60 current timestep. 

61 state : N x 1 numpy array 

62 current state. 

63 *ctrl_args : tuple 

64 Additional arguments to pass to the control model. 

65 

66 Returns 

67 ------- 

68 N x Nu numpy array 

69 Control input matrix. 

70 """ 

71 if self._control_model is None: 

72 raise RuntimeWarning("Control model is not set.") 

73 self.args_to_params((0.1,), ctrl_args) 

74 return self._control_model.get_input_mat(timestep, self.__controlParams) 

75 

76 def args_to_params(self, state_args, control_args): 

77 if len(state_args) != 1: 

78 raise RuntimeError( 

79 "state args must be only (dt,) not {}".format(repr(state_args)) 

80 ) 

81 

82 if len(control_args) != 0 and self._control_model is None: 

83 warn("Control agruments supplied but no control model specified") 

84 elif self._control_model is not None: 

85 self.__controlParams = self._control_model.args_to_params( 

86 tuple(control_args) 

87 ) 

88 

89 self.__constraintParams = cpp_bindings.ConstraintParams() 

90 

91 # hack since state params is empty but things are set in the model 

92 self.__model.dt = state_args[0] 

93 return self.__stateTransParams, self.__controlParams, self.__constraintParams 

94 

95 @property 

96 def model(self): 

97 return self.__model 

98 

99 @property 

100 def state_names(self): 

101 return self.__model.state_names() 

102 

103 def propagate_state(self, timestep, state, u=None, state_args=None, ctrl_args=None): 

104 if state_args is None: 

105 raise RuntimeError("state_args must be (dt,) not None") 

106 self.__model.dt = state_args[0] 

107 if self._control_model is None: 

108 next_state = self.__model.propagate_state(timestep, state).reshape((-1, 1)) 

109 else: 

110 next_state = self.__model.propagate_state( 

111 timestep, state, u, *self.args_to_params(state_args, ctrl_args) 

112 ) 

113 if self.state_constraint is not None: 

114 next_state = self.state_constraint(timestep, next_state) 

115 return next_state.reshape((-1, 1)) 

116 

117 def get_dis_process_noise_mat(self, dt): 

118 """Calculates the process noise. 

119 

120 Parameters 

121 ---------- 

122 dt : float 

123 time difference, not used but needed for standardized interface. 

124 

125 Returns 

126 ------- 

127 6 x 6 numpy array 

128 discrete time process noise matrix. 

129 """ 

130 return np.zeros((len(self.state_names), len(self.state_names))) 

131 

132 def get_state_mat(self, timestep, dt): 

133 """Calculates the state transition matrix. 

134 

135 Parameters 

136 ---------- 

137 timestep : float 

138 current timestep. 

139 dt : float 

140 time difference. 

141 

142 Returns 

143 ------- 

144 F : 6 x 6 numpy array 

145 state transition matrix. 

146 

147 """ 

148 self.__model.dt = dt 

149 return self.__model.get_state_mat(timestep, self.__stateTransParams) 

150 # n = self.mean_motion 

151 # c_dtn = np.cos(dt * n) 

152 # s_dtn = np.sin(dt * n) 

153 # F = np.zeros((6, 6)) 

154 # F2d = super().get_state_mat(timestep, dt) 

155 # F[:2, :2] = F2d[:2, :2] 

156 # F[:2, 3:5] = F2d[:2, 2:] 

157 # F[3:5, :2] = F2d[2:, :2] 

158 # F[3:5, 3:5] = F2d[2:, 2:] 

159 # F[2, 2] = c_dtn 

160 # F[2, 5] = s_dtn / n 

161 # F[5, 2] = -n * s_dtn 

162 # F[5, 5] = c_dtn 

163 # return F