Coverage for src/gncpy/dynamics/basic/irobot_create.py: 94%

1import numpy as np 

2from .nonlinear_dynamics_base import NonlinearDynamicsBase 

3 

4 

5class IRobotCreate(NonlinearDynamicsBase): 

6 """A differential drive robot based on the iRobot Create. 

7 

8 This has a control model predefined because the dynamics themselves do not 

9 change the state. 

10 

11 Notes 

12 ----- 

13 This is taken from :cite:`Berg2016_ExtendedLQRLocallyOptimalFeedbackControlforSystemswithNonLinearDynamicsandNonQuadraticCost` 

14 It represents a 2 wheel robot with some distance between its wheels. 

15 """ 

16 

17 state_names = ("pos_x", "pos_v", "turn_angle") 

18 

19 def __init__(self, wheel_separation=0.258, radius=0.335 / 2, **kwargs): 

20 """Initialize an object. 

21 

22 Parameters 

23 ---------- 

24 wheel_separation : float, optional 

25 Distance between the two wheels in meters. 

26 radius : float 

27 Radius of the bounding box for the robot in meters. 

28 **kwargs : dict 

29 Additional arguments for the parent class. 

30 """ 

31 super().__init__(**kwargs) 

32 self._wheel_separation = wheel_separation 

33 self.radius = radius 

34 

35 def g0(t, x, u, *args): 

36 return 0.5 * ((u[0] + u[1]) * np.cos(x[2])).item() 

37 

38 def g1(t, x, u, *args): 

39 return 0.5 * ((u[0] + u[1]) * np.sin(x[2])).item() 

40 

41 def g2(t, x, u, *args): 

42 return (u[1] - u[0]) / self.wheel_separation 

43 

44 self._control_model = [g0, g1, g2] 

45 

46 @property 

47 def control_model(self): 

48 return self._control_model 

49 

50 @control_model.setter 

51 def control_model(self, model): 

52 self._control_model = model 

53 

54 @property 

55 def wheel_separation(self): 

56 """Read only wheel separation distance.""" 

57 return self._wheel_separation 

58 

59 @property 

60 def cont_fnc_lst(self): 

61 """Implements the contiuous time dynamics.""" 

62 

63 def f0(t, x, *args): 

64 return 0 

65 

66 def f1(t, x, *args): 

67 return 0 

68 

69 def f2(t, x, *args): 

70 return 0 

71 

72 return [f0, f1, f2]