Coverage for src/gncpy/dynamics/basic/double_integrator.py: 86%

1import numpy as np 

2import gncpy.control._control as cpp_control 

3import gncpy.dynamics._dynamics as cpp_bindings 

4from warnings import warn 

5 

6from .linear_dynamics_base import LinearDynamicsBase 

7 

8 

9class DoubleIntegrator(LinearDynamicsBase): 

10 """Implements a double integrator model. 

11 

12 Todo 

13 ---- 

14 Implement the control model in c++ for this class 

15 """ 

16 

17 def __init__(self, **kwargs): 

18 super().__init__(**kwargs) 

19 self.__controlParams = cpp_control.ControlParams() 

20 self.__stateTransParams = cpp_bindings.StateTransParams() 

21 self.__model = cpp_bindings.DoubleIntegrator(0.1) 

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

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

24 

25 @property 

26 def allow_cpp(self): 

27 return True 

28 

29 @property 

30 def control_model(self): 

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

32 return None 

33 

34 @control_model.setter 

35 def control_model(self, model): 

36 if isinstance(model, cpp_control.ILinearControlModel): 

37 self._control_model = model 

38 self.__model.set_control_model(self._control_model) 

39 else: 

40 raise TypeError("must be ILinearControlModel type") 

41 

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

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

44 

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

46 is specified than it returns a zero matrix. 

47 

48 Parameters 

49 ---------- 

50 timestep : float 

51 current timestep. 

52 state : N x 1 numpy array 

53 current state. 

54 *ctrl_args : tuple 

55 Additional arguments to pass to the control model. 

56 

57 Returns 

58 ------- 

59 N x Nu numpy array 

60 Control input matrix. 

61 """ 

62 if self._control_model is None: 

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

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

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

66 

67 # must be provided if allow_cpp is true 

68 def args_to_params(self, state_args, control_args): 

69 if len(state_args) != 1: 

70 raise RuntimeError( 

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

72 ) 

73 

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

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

76 elif self._control_model is not None: 

77 self.__controlParams = self._control_model.args_to_params( 

78 tuple(control_args) 

79 ) 

80 

81 self.__constraintParams = cpp_bindings.ConstraintParams() 

82 

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

84 self.__model.dt = state_args[0] 

85 return self.__stateTransParams, self.__controlParams, self.__constraintParams 

86 

87 # must be provided if allow_cpp is true 

88 @property 

89 def model(self): 

90 return self.__model 

91 

92 @property 

93 def state_names(self): 

94 return self.__model.state_names() 

95 

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

97 if state_args is None: 

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

99 self.__model.dt = state_args[0] 

100 if self._control_model is None: 

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

102 else: 

103 next_state = self.__model.propagate_state( 

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

105 ) 

106 if self.state_constraint is not None: 

107 next_state = self.state_constraint(timestep, next_state) 

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

109 

110 def get_dis_process_noise_mat(self, dt, proc_cov): 

111 """Discrete process noise matrix. 

112 

113 Parameters 

114 ---------- 

115 dt : float 

116 time difference, unused. 

117 proc_cov : N x N numpy array 

118 Covariance matrix of the process noise. 

119 

120 Returns 

121 ------- 

122 4 x 4 numpy array 

123 process noise matrix. 

124 

125 """ 

126 gamma = np.array([0, 0, 1, 1]).reshape((4, 1)) 

127 return gamma @ proc_cov @ gamma.T 

128 

129 def get_state_mat(self, timestep, dt): 

130 """Class method for getting the discrete time state matrix. 

131 

132 Parameters 

133 ---------- 

134 timestep : float 

135 timestep. 

136 dt : float 

137 time difference 

138 

139 Returns 

140 ------- 

141 4 x 4 numpy array 

142 state matrix. 

143 

144 """ 

145 self.__model.dt = dt 

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