From Gamboge Hamster, 2 Months ago, written in Python.
This paste is a reply to subfunctions from Toxic Pheasant - view diff
Embed
Hits: 46
1. import numpy as np
2. import math as math
3. from scipy.integrate import solve_ivp, simps
4. from scipy.interpolate import interp1d
5. from scipy.integrate import simps
6.
7. def define_rover_1():   # or define_rover()
8.     # Initialize Rover dict for testing
10.              'mass':1}
11.     speed_reducer = {'type':'reverted',
12.                      'diam_pinion':0.04,
13.                      'diam_gear':0.07,
14.                      'mass':1.5}
15.     motor = {'torque_stall':170,
18.              'mass':5.0}
19.
20.
21.     chassis = {'mass':659}
23.     power_subsys = {'mass':90}
24.
25.     wheel_assembly = {'wheel':wheel,
26.                       'speed_reducer':speed_reducer,
27.                       'motor':motor}
28.
29.     rover = {'wheel_assembly':wheel_assembly,
30.              'chassis':chassis,
32.              'power_subsys':power_subsys}
33.
34.     planet = {'g':3.72}
35.
36.     # return everything we need
37.     return rover, planet
38.
39. def get_mass(rover):
40.     """
41.    Inputs:  rover:  dict      Data structure containing rover parameters
42.
43.    Outputs:     m:  scalar    Rover mass [kg].
44.    """
45.
46.     # Check that the input is a dict
47.     if type(rover) != dict:
48.         raise Exception('Input must be a dict')
49.
51.     # and components from all six wheel assemblies
52.     m = rover['chassis']['mass'] \
53.         + rover['power_subsys']['mass'] \
55.         + 6*rover['wheel_assembly']['motor']['mass'] \
56.         + 6*rover['wheel_assembly']['speed_reducer']['mass'] \
57.         + 6*rover['wheel_assembly']['wheel']['mass'] \
58.
59.     return m
60.
61.
62. def get_gear_ratio(speed_reducer):
63.     """
64.    Inputs:  speed_reducer:  dict      Data dictionary specifying speed
65.                                        reducer parameters
66.    Outputs:            Ng:  scalar    Speed ratio from input pinion shaft
67.                                        to output gear shaft. Unitless.
68.    """
69.
70.     # Check that 1 input has been given.
71.     #   IS THIS NECESSARY ANYMORE????
72.
73.     # Check that the input is a dict
74.     if type(speed_reducer) != dict:
75.         raise Exception('Input must be a dict')
76.
77.     # Check 'type' field (not case sensitive)
78.     if speed_reducer['type'].lower() != 'reverted':
79.         raise Exception('The speed reducer type is not recognized.')
80.
81.     # Main code
82.     d1 = speed_reducer['diam_pinion']
83.     d2 = speed_reducer['diam_gear']
84.
85.     Ng = (d2/d1)**2
86.
87.     return Ng
88.
89.
90. def tau_dcmotor(omega, motor):
91.     """
92.    Inputs:  omega:  numpy array      Motor shaft speed [rad/s]
93.             motor:  dict             Data dictionary specifying motor parameters
94.    Outputs:   tau:  numpy array      Torque at motor shaft [Nm].  Return argument
95.                                      is same size as first input argument.
96.    """
97.
98.     # Check that 2 inputs have been given
99.     #   IS THIS NECESSARY ANYMORE????
100.
101.     # Check that the first input is a scalar or a vector
102.     if (type(omega) != int) and (type(omega) != float) and (not isinstance(omega, np.ndarray)):
103.         raise Exception ('First input must be a scalar or a vector. If input is a vector, it should be defined as a numpy array.')
104.     elif not isinstance(omega, np.ndarray):
105.         omega = np.array([omega],dtype=float) # make the scalar a numpy array
106.     elif len(np.shape(omega)) != 1:
107.         raise Exception('First input must be a scalar or a vector. Matrices are not allowed.')
108.
109.     # Check that the second input is a dict
110.     if type(motor) != dict:
111.         raise Exception('Second input must be a dict')
112.
113.     # Main code
114.     tau_s    = motor['torque_stall']
117.
118.     # initialize
119.     tau = np.zeros(len(omega),dtype = float)
120.     for i in range(len(omega)):
121.         if omega[i] >= 0 and omega[i] <= omega_nl:
122.             tau[i] = tau_s - (tau_s-tau_nl)/omega_nl *omega[i]
123.         elif omega[i] < 0:
124.             tau[i] = tau_s
125.         elif omega[i] > omega_nl:
126.             tau[i] = 0
127.
128.     return tau
129.
130.
131.
132.
133. def F_rolling(omega, terrain_angle, rover, planet, Crr):
134.     """
135.    Inputs:           omega:  numpy array     Motor shaft speed [rad/s]
136.              terrain_angle:  numpy array     Array of terrain angles [deg]
137.                      rover:  dict            Data structure specifying rover
138.                                              parameters
139.                    planet:  dict            Data dictionary specifying planetary
140.                                              parameters
141.                        Crr:  scalar          Value of rolling resistance coefficient
142.                                              [-]
143.
144.    Outputs:           Frr:  numpy array     Array of forces [N]
145.    """
146.
147.     # Check that the first input is a scalar or a vector
148.     if (type(omega) != int) and (type(omega) != float) and (not isinstance(omega, np.ndarray)):
149.         raise Exception('First input must be a scalar or a vector. If input is a vector, it should be defined as a numpy array.')
150.     elif not isinstance(omega, np.ndarray):
151.         omega = np.array([omega],dtype=float) # make the scalar a numpy array
152.     elif len(np.shape(omega)) != 1:
153.         raise Exception('First input must be a scalar or a vector. Matrices are not allowed.')
154.
155.     # Check that the second input is a scalar or a vector
156.     if (type(terrain_angle) != int) and (type(terrain_angle) != float) and (not isinstance(terrain_angle, np.ndarray)):
157.         raise Exception('Second input must be a scalar or a vector. If input is a vector, it should be defined as a numpy array.')
158.     elif not isinstance(terrain_angle, np.ndarray):
159.         terrain_angle = np.array([terrain_angle],dtype=float) # make the scalar a numpy array
160.     elif len(np.shape(terrain_angle)) != 1:
161.         raise Exception('Second input must be a scalar or a vector. Matrices are not allowed.')
162.
163.     # Check that the first two inputs are of the same size
164.     if len(omega) != len(terrain_angle):
165.         raise Exception('First two inputs must be the same size')
166.
167.     # Check that values of the second input are within the feasible range
168.     if max([abs(x) for x in terrain_angle]) > 75:
169.         raise Exception('All elements of the second input must be between -75 degrees and +75 degrees')
170.
171.     # Check that the third input is a dict
172.     if type(rover) != dict:
173.         raise Exception('Third input must be a dict')
174.
175.     # Check that the fourth input is a dict
176.     if type(planet) != dict:
177.         raise Exception('Fourth input must be a dict')
178.
179.     # Check that the fifth input is a scalar and positive
180.     if (type(Crr) != int) and (type(Crr) != float):
181.         raise Exception('Fifth input must be a scalar')
182.     if Crr <= 0:
183.         raise Exception('Fifth input must be a positive number')
184.
185.     # Main Code
186.     m = get_mass(rover)
187.     g = planet['g']
189.     Ng = get_gear_ratio(rover['wheel_assembly']['speed_reducer'])
190.
191.     v_rover = r*omega/Ng
192.
193.     Fn = np.array([m*g*math.cos(math.radians(x)) for x in terrain_angle],dtype=float) # normal force
194.     Frr_simple = -Crr*Fn # simple rolling resistance
195.
196.     Frr = np.array([math.erf(40*v_rover[ii]) * Frr_simple[ii] for ii in range(len(v_rover))], dtype = float)
197.
198.     return Frr
199.
200.
201. def F_gravity(terrain_angle, rover, planet):
202.     """
203.    Inputs:  terrain_angle:  numpy array   Array of terrain angles [deg]
204.                     rover:  dict          Data structure specifying rover
205.                                            parameters
206.                    planet:  dict          Data dictionary specifying planetary
207.                                            parameters
208.
209.    Outputs:           Fgt:  numpy array   Array of forces [N]
210.    """
211.
212.     # Check that the first input is a scalar or a vector
213.     if (type(terrain_angle) != int) and (type(terrain_angle) != float) and (not isinstance(terrain_angle, np.ndarray)):
214.         raise Exception('First input must be a scalar or a vector. If input is a vector, it should be defined as a numpy array.')
215.     elif not isinstance(terrain_angle, np.ndarray):
216.         terrain_angle = np.array([terrain_angle],dtype=float) # make the scalar a numpy array
217.     elif len(np.shape(terrain_angle)) != 1:
218.         raise Exception('First input must be a scalar or a vector. Matrices are not allowed.')
219.
220.     # Check that values of the first input are within the feasible range
221.     if max([abs(x) for x in terrain_angle]) > 75:
222.         raise Exception('All elements of the first input must be between -75 degrees and +75 degrees')
223.
224.     # Check that the second input is a dict
225.     if type(rover) != dict:
226.         raise Exception('Second input must be a dict')
227.
228.     # Check that the third input is a dict
229.     if type(planet) != dict:
230.         raise Exception('Third input must be a dict')
231.
232.     # Main Code
233.     m = get_mass(rover)
234.     g = planet['g']
235.
236.     Fgt = np.array([-m*g*math.sin(math.radians(x)) for x in terrain_angle], dtype = float)
237.
238.     return Fgt
239.
240.
241. def F_drive(omega, rover):
242.     """
243.    Inputs:  omega:  numpy array   Array of motor shaft speeds [rad/s]
244.             rover:  dict          Data dictionary specifying rover parameters
245.
246.    Outputs:    Fd:  numpy array   Array of drive forces [N]
247.    """
248.
249.     # Check that 2 inputs have been given.
250.     #   IS THIS NECESSARY ANYMORE????
251.
252.     # Check that the first input is a scalar or a vector
253.     if (type(omega) != int) and (type(omega) != float) and (not isinstance(omega, np.ndarray)):
254.         raise Exception('First input must be a scalar or a vector. If input is a vector, it should be defined as a numpy array.')
255.     elif not isinstance(omega, np.ndarray):
256.         omega = np.array([omega],dtype=float) # make the scalar a numpy array
257.     elif len(np.shape(omega)) != 1:
258.         raise Exception('First input must be a scalar or a vector. Matrices are not allowed.')
259.
260.     # Check that the second input is a dict
261.     if type(rover) != dict:
262.         raise Exception('Second input must be a dict')
263.
264.     # Main code
265.     Ng = get_gear_ratio(rover['wheel_assembly']['speed_reducer'])
266.
267.     tau = tau_dcmotor(omega, rover['wheel_assembly']['motor'])
268.     tau_out = tau*Ng
269.
271.
272.     # Drive force for one wheel
273.     Fd_wheel = tau_out/r
274.
275.     # Drive force for all six wheels
276.     Fd = 6*Fd_wheel
277.
278.     return Fd
279.
280.
281. def F_net(omega, terrain_angle, rover, planet, Crr):
282.     """
283.    Inputs:           omega:  list     Motor shaft speed [rad/s]
284.              terrain_angle:  list     Array of terrain angles [deg]
285.                      rover:  dict     Data structure specifying rover
286.                                      parameters
287.                     planet:  dict     Data dictionary specifying planetary
288.                                      parameters
289.                        Crr:  scalar   Value of rolling resistance coefficient
290.                                      [-]
291.
292.    Outputs:           Fnet:  list     Array of forces [N]
293.    """
294.
295.     # Check that the first input is a scalar or a vector
296.     if (type(omega) != int) and (type(omega) != float) and (not isinstance(omega, np.ndarray)):
297.         raise Exception('First input must be a scalar or a vector. If input is a vector, it should be defined as a numpy array.')
298.     elif not isinstance(omega, np.ndarray):
299.         omega = np.array([omega],dtype=float) # make the scalar a numpy array
300.     elif len(np.shape(omega)) != 1:
301.         raise Exception('First input must be a scalar or a vector. Matrices are not allowed.')
302.
303.     # Check that the second input is a scalar or a vector
304.     if (type(terrain_angle) != int) and (type(terrain_angle) != float) and (not isinstance(terrain_angle, np.ndarray)):
305.         raise Exception('Second input must be a scalar or a vector. If input is a vector, it should be defined as a numpy array.')
306.     elif not isinstance(terrain_angle, np.ndarray):
307.         terrain_angle = np.array([terrain_angle],dtype=float) # make the scalar a numpy array
308.     elif len(np.shape(terrain_angle)) != 1:
309.         raise Exception('Second input must be a scalar or a vector. Matrices are not allowed.')
310.
311.     # Check that the first two inputs are of the same size
312.     if len(omega) != len(terrain_angle):
313.         raise Exception('First two inputs must be the same size')
314.
315.     # Check that values of the second input are within the feasible range
316.     if max([abs(x) for x in terrain_angle]) > 75:
317.         raise Exception('All elements of the second input must be between -75 degrees and +75 degrees')
318.
319.     # Check that the third input is a dict
320.     if type(rover) != dict:
321.         raise Exception('Third input must be a dict')
322.
323.     # Check that the fourth input is a dict
324.     if type(planet) != dict:
325.         raise Exception('Fourth input must be a dict')
326.
327.     # Check that the fifth input is a scalar and positive
328.     if (type(Crr) != int) and (type(Crr) != float):
329.         raise Exception('Fifth input must be a scalar')
330.     if Crr <= 0:
331.         raise Exception('Fifth input must be a positive number')
332.
333.     # Main Code
334.     Fd = F_drive(omega, rover)
335.     Frr = F_rolling(omega, terrain_angle, rover, planet, Crr)
336.     Fg = F_gravity(terrain_angle, rover, planet)
337.
338.     Fnet = Fd + Frr + Fg # signs are handled in individual functions
339.
340.     return Fnet
341.
342. def motorW(v, rover):
343.     '''
344.    General Description:
345.    Compute the rotational speed of the motor shaft [rad/s] given the translational velocity of the rover and the rover
346.    dictionary.This function should be “vectorized” such that if given a vector of rover velocities it returns a vector the same size
347.    containing the corresponding motor speeds.
348.
349.    Calling Syntax
350.         w = motorW(v, rover)
351.    Input Arguments
352.         v -- 1D numpy array OR scalar float/int -- Rover translational velocity [m/s]
353.         rover -- dictionary -- Data structure containing rover parameters
354.    Return Arguments
355.         w -- 1D numpy array OR scalar float/int -- Motor speed [rad/s]. Return argument should be the same size as the input argument v.
357.        ▪ This function should validate (a) that the first input is a scalar or vector and (b) that the second input is a
358.        dictionary. If any condition fails, call raise Exception().
359.        ▪ This function should call get_gear_ratio from Phase 1.
360.        ▪ Be wary of units.
361.    '''
362.
363.     # Validate input arguments
364.     assert isinstance(v, (np.ndarray, float, int)), "v must be a scalar or 1D numpy array"
365.     assert isinstance(rover, dict), "rover must be a dictionary"
366.
367.     # Store frequently used parameters in variables
369.     Ng = get_gear_ratio(rover['wheel_assembly']['speed_reducer'])
370.
371.     # Compute motor speed
372.     w = v / radius * Ng
373.
374.     return w
375.
376. def rover_dynamics(t, y, rover, planet, experiment):
377.     '''
378.    rover_dynamics
379.    General Description
380.    This function computes the derivative of the state vector (state vector is: [velocity, position]) for the rover given its
381.    current state. It requires rover and experiment dictionary input parameters. It is intended to be passed to an ODE
382.    solver.
383.    '''
384.
385.     # Validates input types and shapes
386.     if not isinstance(t, (float, int, np.float64, np.int32)):
387.         raise TypeError("t must be a scalar")
388.     if not isinstance(y, np.ndarray) or y.shape != (2,):
389.         raise TypeError("y must be a 1D numpy array of length 2")
390.     if not isinstance(rover, dict):
391.         raise TypeError("rover must be a dictionary")
392.     if not isinstance(planet, dict):
393.         raise TypeError("planet must be a dictionary")
394.     if not isinstance(experiment, dict):
395.         raise TypeError("experiment must be a dictionary")
396.
397.     # Computes terrain angle using cubic spline interpolation
398.     alpha_fun = interp1d(experiment['alpha_dist'], experiment['alpha_deg'], kind='cubic', fill_value='extrapolate')
399.     terrain_angle = alpha_fun(y[1])
400.
401.     # Computes motor speed
402.     w = motorW(y[0], rover)
403.
404.     # Computes net force using previously computed terrain angle
405.     Fnet = F_net(w, terrain_angle, rover, planet, experiment['Crr'])
406.
407.     # Computes acceleration and velocity
408.     mrover = get_mass(rover)
409.     a = Fnet / mrover
410.     dydt = np.array([a, y[0]])
411.
412.     return dydt
413.
414. def mechpower(v, rover):
415.
416.     power = []
417.
418.     # Exceptions
419.     if type(rover) is bool:
420.         raise Exception("V must be scalar or vector input")
421.     if type(v) != dict:
422.         raise Exception("Rover must be a dictionary input")
423.
424.     if np.isscalar(v):
425.         w = motorW(v,rover)
426.         omega = w
427.         tau = tau_dcmotor(omega, motor)
428.         pwr = tau * w
429.         p = pwr
430.     else:
431.         for i in range(len(v)):
432.             w = motorW(v[i],rover)
433.             omega = wtau = tau_dcmotor(omega, motor)
434.             pwr = tau * w
435.             power.append(pwr)
436.             p = np.array(power)
437.     return p
438.
439.
440. def battenergy(t, v, rover):
441.
442.     #Exceptions
443.     if (type(t) is bool):
444.         raise Exception('t must be a scalar or vector input')
445.     if type(t) != type(v):
446.         raise Exception('t and v must be the same type')
447.     if type(v) is bool:
448.         raise Exception('v must be a scalar or vector input')
449.     if np.size(t) != np.size(v):
450.         raise Exception('v and t must be the same size')
451.     if (type(rover) is not dict):
452.         raise Exception('rover must be a dictionary input')
453.
454.     p = mechpower(v, rover)
455.     w = motorW(v, rover)
456.     omega = w
457.     tau = tau_dcmotor(omega, motor)
458.     effcy_tau = motor['effcy_tau']
459.     efccy = motor['effcy']
460.     effcy_fun = interpolate.interp1d(effcy_tau, effcy, kind='cubic', fill_value='extrapolate')
461.     bpower = (6*p) / (effcy_fun(tau))
462.     E = integrate.trapz(bpower,t)
463.     return E
464.
465. #############################################
466. def end_of_mission_event(end_event):
467.     """
468.    Defines an event that terminates the mission simulation. Mission is over
469.    when rover reaches a certain distance, has moved for a maximum simulation
470.    time or has reached a minimum velocity.
471.    """
472.
473.     mission_distance = end_event['max_distance']
474.     mission_max_time = end_event['max_time']
475.     mission_min_velocity = end_event['min_velocity']
476.
477.     # Assume that y[1] is the distance traveled
478.     distance_left = lambda t,y: mission_distance - y[1]
479.     distance_left.terminal = True
480.
481.     time_left = lambda t,y: mission_max_time - t
482.     time_left.terminal = True
483.
484.     velocity_threshold = lambda t,y: y[0] - mission_min_velocity;
485.     velocity_threshold.terminal = True
486.     velocity_threshold.direction = -1
487.
488.     # terminal indicates whether any of the conditions can lead to the
489.     # termination of the ODE solver. In this case all conditions can terminate
490.     # the simulation independently.
491.
492.     # direction indicates whether the direction along which the different
493.     # conditions is reached matter or does not matter. In this case, only
494.     # the direction in which the velocity treshold is arrived at matters
495.     # (negative)
496.
497.     events = [distance_left, time_left, velocity_threshold]
498.
499.     return events
500.
501. def experiment1():
502.
503.     experiment = {'time_range' : np.array([0,20000]),
504.                   'initial_conditions' : np.array([0.3125,0]),
505.                   'alpha_dist' : np.array([0, 100, 200, 300, 400, 500, 600, 700, 800, 900, 1000]),
506.                   'alpha_deg' : np.array([11.509, 2.032, 7.182, 2.478, \
507.                                         5.511, 10.981, 5.601, -0.184, \
508.                                         0.714, 4.151, 4.042]),
509.                   'Crr' : 0.1}
510.
511.
512.     # Below are default values for example only:
513.     end_event = {'max_distance' : 50,
514.                  'max_time' : 5000,
515.                  'min_velocity' : 0.01}
516.
517.     return experiment, end_event
518.
519. def simulate_rover(rover, planet, experiment, end_event):
520.     '''
521.    Simulates the trajectory of a rover on a planet surface and returns updated telemetry information.
522.
523.    Inputs:
524.    rover (dict): A dictionary containing the parameters of the rover.
525.    planet (dict): A dictionary containing the planet definition.
526.    experiment (dict): A dictionary containing parameters of the trajectory to be followed by the rover.
527.    end_event (dict): A dictionary containing the conditions necessary and sufficient to terminate simulation of rover dynamics.
528.
529.    Returns:
530.    rover (dict): A dictionary containing the parameters of the rover, including updated telemetry information.
531.    '''
532.
533.     # Check inputs are dictionaries
534.     if not isinstance(experiment, dict):
535.         raise TypeError('Input must be a dictionary')
536.     if not isinstance(end_event, dict):
537.         raise TypeError('Input must be a dictionary')
538.
539.     # Set initial conditions and differential equation to solve
540.     t_span = experiment["time_range"]
541.     y0 = experiment["initial_conditions"]
542.     function = lambda time, y: rover_dynamics(time, y, rover, planet, experiment)
543.
544.     # Solve the differential equation and return solution
545.     solution = solve_ivp(function, t_span=t_span, y0=y0, method='RK45', events=end_of_mission_event(end_event))
546.
547.     # Update rover telemetry
548.     rover['telemetry']['Time'] = solution.t
549.     rover['telemetry']['completion_time'] = solution.t[-1]
550.     rover['telemetry']['velocity'] = solution.y[0]
551.     rover['telemetry']['position'] = solution.y[1]
552.     rover['telemetry']['distance_traveled'] = solution.y[1][-1]
553.     rover['telemetry']['max_velocity'] = np.max(solution.y[0])
554.     rover['telemetry']['average_velocity'] = np.mean(solution.y[0])
555.     rover['telemetry']['power'] = mechpower(solution.y[0], rover)
556.     rover['telemetry']['battery_energy'] = battenergy(solution.t, solution.y[0], rover)
557.     rover['telemetry']['energy_per_distance'] = rover['telemetry']['battery_energy'] / rover['telemetry']['distance_traveled']
558.
559.     return rover
560.