import numpy as np import math as math from scipy.integrate import solve_ivp, simps from scipy.interpolate import interp1d from scipy.integrate import simps def define_rover_1(): # or define_rover() # Initialize Rover dict for testing wheel = {'radius':0.30, 'mass':1} speed_reducer = {'type':'reverted', 'diam_pinion':0.04, 'diam_gear':0.07, 'mass':1.5} motor = {'torque_stall':170, 'torque_noload':0, 'speed_noload':3.80, 'mass':5.0} chassis = {'mass':659} science_payload = {'mass':75} power_subsys = {'mass':90} wheel_assembly = {'wheel':wheel, 'speed_reducer':speed_reducer, 'motor':motor} rover = {'wheel_assembly':wheel_assembly, 'chassis':chassis, 'science_payload':science_payload, 'power_subsys':power_subsys} planet = {'g':3.72} # return everything we need return rover, planet def get_mass(rover): """ Inputs: rover: dict Data structure containing rover parameters Outputs: m: scalar Rover mass [kg]. """ # Check that the input is a dict if type(rover) != dict: raise Exception('Input must be a dict') # add up mass of chassis, power subsystem, science payload, # and components from all six wheel assemblies m = rover['chassis']['mass'] \ + rover['power_subsys']['mass'] \ + rover['science_payload']['mass'] \ + 6*rover['wheel_assembly']['motor']['mass'] \ + 6*rover['wheel_assembly']['speed_reducer']['mass'] \ + 6*rover['wheel_assembly']['wheel']['mass'] \ return m def get_gear_ratio(speed_reducer): """ Inputs: speed_reducer: dict Data dictionary specifying speed reducer parameters Outputs: Ng: scalar Speed ratio from input pinion shaft to output gear shaft. Unitless. """ # Check that 1 input has been given. # IS THIS NECESSARY ANYMORE???? # Check that the input is a dict if type(speed_reducer) != dict: raise Exception('Input must be a dict') # Check 'type' field (not case sensitive) if speed_reducer['type'].lower() != 'reverted': raise Exception('The speed reducer type is not recognized.') # Main code d1 = speed_reducer['diam_pinion'] d2 = speed_reducer['diam_gear'] Ng = (d2/d1)**2 return Ng def tau_dcmotor(omega, motor): """ Inputs: omega: numpy array Motor shaft speed [rad/s] motor: dict Data dictionary specifying motor parameters Outputs: tau: numpy array Torque at motor shaft [Nm]. Return argument is same size as first input argument. """ # Check that 2 inputs have been given # IS THIS NECESSARY ANYMORE???? # Check that the first input is a scalar or a vector if (type(omega) != int) and (type(omega) != float) and (not isinstance(omega, np.ndarray)): raise Exception ('First input must be a scalar or a vector. If input is a vector, it should be defined as a numpy array.') elif not isinstance(omega, np.ndarray): omega = np.array([omega],dtype=float) # make the scalar a numpy array elif len(np.shape(omega)) != 1: raise Exception('First input must be a scalar or a vector. Matrices are not allowed.') # Check that the second input is a dict if type(motor) != dict: raise Exception('Second input must be a dict') # Main code tau_s = motor['torque_stall'] tau_nl = motor['torque_noload'] omega_nl = motor['speed_noload'] # initialize tau = np.zeros(len(omega),dtype = float) for i in range(len(omega)): if omega[i] >= 0 and omega[i] <= omega_nl: tau[i] = tau_s - (tau_s-tau_nl)/omega_nl *omega[i] elif omega[i] < 0: tau[i] = tau_s elif omega[i] > omega_nl: tau[i] = 0 return tau def F_rolling(omega, terrain_angle, rover, planet, Crr): """ Inputs: omega: numpy array Motor shaft speed [rad/s] terrain_angle: numpy array Array of terrain angles [deg] rover: dict Data structure specifying rover parameters planet: dict Data dictionary specifying planetary parameters Crr: scalar Value of rolling resistance coefficient [-] Outputs: Frr: numpy array Array of forces [N] """ # Check that the first input is a scalar or a vector if (type(omega) != int) and (type(omega) != float) and (not isinstance(omega, np.ndarray)): raise Exception('First input must be a scalar or a vector. If input is a vector, it should be defined as a numpy array.') elif not isinstance(omega, np.ndarray): omega = np.array([omega],dtype=float) # make the scalar a numpy array elif len(np.shape(omega)) != 1: raise Exception('First input must be a scalar or a vector. Matrices are not allowed.') # Check that the second input is a scalar or a vector if (type(terrain_angle) != int) and (type(terrain_angle) != float) and (not isinstance(terrain_angle, np.ndarray)): raise Exception('Second input must be a scalar or a vector. If input is a vector, it should be defined as a numpy array.') elif not isinstance(terrain_angle, np.ndarray): terrain_angle = np.array([terrain_angle],dtype=float) # make the scalar a numpy array elif len(np.shape(terrain_angle)) != 1: raise Exception('Second input must be a scalar or a vector. Matrices are not allowed.') # Check that the first two inputs are of the same size if len(omega) != len(terrain_angle): raise Exception('First two inputs must be the same size') # Check that values of the second input are within the feasible range if max([abs(x) for x in terrain_angle]) > 75: raise Exception('All elements of the second input must be between -75 degrees and +75 degrees') # Check that the third input is a dict if type(rover) != dict: raise Exception('Third input must be a dict') # Check that the fourth input is a dict if type(planet) != dict: raise Exception('Fourth input must be a dict') # Check that the fifth input is a scalar and positive if (type(Crr) != int) and (type(Crr) != float): raise Exception('Fifth input must be a scalar') if Crr <= 0: raise Exception('Fifth input must be a positive number') # Main Code m = get_mass(rover) g = planet['g'] r = rover['wheel_assembly']['wheel']['radius'] Ng = get_gear_ratio(rover['wheel_assembly']['speed_reducer']) v_rover = r*omega/Ng Fn = np.array([m*g*math.cos(math.radians(x)) for x in terrain_angle],dtype=float) # normal force Frr_simple = -Crr*Fn # simple rolling resistance Frr = np.array([math.erf(40*v_rover[ii]) * Frr_simple[ii] for ii in range(len(v_rover))], dtype = float) return Frr def F_gravity(terrain_angle, rover, planet): """ Inputs: terrain_angle: numpy array Array of terrain angles [deg] rover: dict Data structure specifying rover parameters planet: dict Data dictionary specifying planetary parameters Outputs: Fgt: numpy array Array of forces [N] """ # Check that the first input is a scalar or a vector if (type(terrain_angle) != int) and (type(terrain_angle) != float) and (not isinstance(terrain_angle, np.ndarray)): raise Exception('First input must be a scalar or a vector. If input is a vector, it should be defined as a numpy array.') elif not isinstance(terrain_angle, np.ndarray): terrain_angle = np.array([terrain_angle],dtype=float) # make the scalar a numpy array elif len(np.shape(terrain_angle)) != 1: raise Exception('First input must be a scalar or a vector. Matrices are not allowed.') # Check that values of the first input are within the feasible range if max([abs(x) for x in terrain_angle]) > 75: raise Exception('All elements of the first input must be between -75 degrees and +75 degrees') # Check that the second input is a dict if type(rover) != dict: raise Exception('Second input must be a dict') # Check that the third input is a dict if type(planet) != dict: raise Exception('Third input must be a dict') # Main Code m = get_mass(rover) g = planet['g'] Fgt = np.array([-m*g*math.sin(math.radians(x)) for x in terrain_angle], dtype = float) return Fgt def F_drive(omega, rover): """ Inputs: omega: numpy array Array of motor shaft speeds [rad/s] rover: dict Data dictionary specifying rover parameters Outputs: Fd: numpy array Array of drive forces [N] """ # Check that 2 inputs have been given. # IS THIS NECESSARY ANYMORE???? # Check that the first input is a scalar or a vector if (type(omega) != int) and (type(omega) != float) and (not isinstance(omega, np.ndarray)): raise Exception('First input must be a scalar or a vector. If input is a vector, it should be defined as a numpy array.') elif not isinstance(omega, np.ndarray): omega = np.array([omega],dtype=float) # make the scalar a numpy array elif len(np.shape(omega)) != 1: raise Exception('First input must be a scalar or a vector. Matrices are not allowed.') # Check that the second input is a dict if type(rover) != dict: raise Exception('Second input must be a dict') # Main code Ng = get_gear_ratio(rover['wheel_assembly']['speed_reducer']) tau = tau_dcmotor(omega, rover['wheel_assembly']['motor']) tau_out = tau*Ng r = rover['wheel_assembly']['wheel']['radius'] # Drive force for one wheel Fd_wheel = tau_out/r # Drive force for all six wheels Fd = 6*Fd_wheel return Fd def F_net(omega, terrain_angle, rover, planet, Crr): """ Inputs: omega: list Motor shaft speed [rad/s] terrain_angle: list Array of terrain angles [deg] rover: dict Data structure specifying rover parameters planet: dict Data dictionary specifying planetary parameters Crr: scalar Value of rolling resistance coefficient [-] Outputs: Fnet: list Array of forces [N] """ # Check that the first input is a scalar or a vector if (type(omega) != int) and (type(omega) != float) and (not isinstance(omega, np.ndarray)): raise Exception('First input must be a scalar or a vector. If input is a vector, it should be defined as a numpy array.') elif not isinstance(omega, np.ndarray): omega = np.array([omega],dtype=float) # make the scalar a numpy array elif len(np.shape(omega)) != 1: raise Exception('First input must be a scalar or a vector. Matrices are not allowed.') # Check that the second input is a scalar or a vector if (type(terrain_angle) != int) and (type(terrain_angle) != float) and (not isinstance(terrain_angle, np.ndarray)): raise Exception('Second input must be a scalar or a vector. If input is a vector, it should be defined as a numpy array.') elif not isinstance(terrain_angle, np.ndarray): terrain_angle = np.array([terrain_angle],dtype=float) # make the scalar a numpy array elif len(np.shape(terrain_angle)) != 1: raise Exception('Second input must be a scalar or a vector. Matrices are not allowed.') # Check that the first two inputs are of the same size if len(omega) != len(terrain_angle): raise Exception('First two inputs must be the same size') # Check that values of the second input are within the feasible range if max([abs(x) for x in terrain_angle]) > 75: raise Exception('All elements of the second input must be between -75 degrees and +75 degrees') # Check that the third input is a dict if type(rover) != dict: raise Exception('Third input must be a dict') # Check that the fourth input is a dict if type(planet) != dict: raise Exception('Fourth input must be a dict') # Check that the fifth input is a scalar and positive if (type(Crr) != int) and (type(Crr) != float): raise Exception('Fifth input must be a scalar') if Crr <= 0: raise Exception('Fifth input must be a positive number') # Main Code Fd = F_drive(omega, rover) Frr = F_rolling(omega, terrain_angle, rover, planet, Crr) Fg = F_gravity(terrain_angle, rover, planet) Fnet = Fd + Frr + Fg # signs are handled in individual functions return Fnet def motorW(v, rover): ''' General Description: Compute the rotational speed of the motor shaft [rad/s] given the translational velocity of the rover and the rover dictionary.This function should be “vectorized” such that if given a vector of rover velocities it returns a vector the same size containing the corresponding motor speeds. Calling Syntax w = motorW(v, rover) Input Arguments v -- 1D numpy array OR scalar float/int -- Rover translational velocity [m/s] rover -- dictionary -- Data structure containing rover parameters Return Arguments w -- 1D numpy array OR scalar float/int -- Motor speed [rad/s]. Return argument should be the same size as the input argument v. Additional Specifications and Notes ▪ This function should validate (a) that the first input is a scalar or vector and (b) that the second input is a dictionary. If any condition fails, call raise Exception(). ▪ This function should call get_gear_ratio from Phase 1. ▪ Be wary of units. ''' # Validate input arguments assert isinstance(v, (np.ndarray, float, int)), "v must be a scalar or 1D numpy array" assert isinstance(rover, dict), "rover must be a dictionary" # Store frequently used parameters in variables radius = rover['wheel_assembly']['wheel']['radius'] Ng = get_gear_ratio(rover['wheel_assembly']['speed_reducer']) # Compute motor speed w = v / radius * Ng return w def rover_dynamics(t, y, rover, planet, experiment): ''' rover_dynamics General Description This function computes the derivative of the state vector (state vector is: [velocity, position]) for the rover given its current state. It requires rover and experiment dictionary input parameters. It is intended to be passed to an ODE solver. ''' # Validates input types and shapes if not isinstance(t, (float, int, np.float64, np.int32)): raise TypeError("t must be a scalar") if not isinstance(y, np.ndarray) or y.shape != (2,): raise TypeError("y must be a 1D numpy array of length 2") if not isinstance(rover, dict): raise TypeError("rover must be a dictionary") if not isinstance(planet, dict): raise TypeError("planet must be a dictionary") if not isinstance(experiment, dict): raise TypeError("experiment must be a dictionary") # Computes terrain angle using cubic spline interpolation alpha_fun = interp1d(experiment['alpha_dist'], experiment['alpha_deg'], kind='cubic', fill_value='extrapolate') terrain_angle = alpha_fun(y[1]) # Computes motor speed w = motorW(y[0], rover) # Computes net force using previously computed terrain angle Fnet = F_net(w, terrain_angle, rover, planet, experiment['Crr']) # Computes acceleration and velocity mrover = get_mass(rover) a = Fnet / mrover dydt = np.array([a, y[0]]) return dydt def mechpower(v, rover): power = [] # Exceptions if type(rover) is bool: raise Exception("V must be scalar or vector input") if type(v) != dict: raise Exception("Rover must be a dictionary input") if np.isscalar(v): w = motorW(v,rover) omega = w tau = tau_dcmotor(omega, motor) pwr = tau * w p = pwr else: for i in range(len(v)): w = motorW(v[i],rover) omega = wtau = tau_dcmotor(omega, motor) pwr = tau * w power.append(pwr) p = np.array(power) return p def battenergy(t, v, rover): #Exceptions if (type(t) is bool): raise Exception('t must be a scalar or vector input') if type(t) != type(v): raise Exception('t and v must be the same type') if type(v) is bool: raise Exception('v must be a scalar or vector input') if np.size(t) != np.size(v): raise Exception('v and t must be the same size') if (type(rover) is not dict): raise Exception('rover must be a dictionary input') p = mechpower(v, rover) w = motorW(v, rover) omega = w tau = tau_dcmotor(omega, motor) effcy_tau = motor['effcy_tau'] efccy = motor['effcy'] effcy_fun = interpolate.interp1d(effcy_tau, effcy, kind='cubic', fill_value='extrapolate') bpower = (6*p) / (effcy_fun(tau)) E = integrate.trapz(bpower,t) return E ############################################# def end_of_mission_event(end_event): """ Defines an event that terminates the mission simulation. Mission is over when rover reaches a certain distance, has moved for a maximum simulation time or has reached a minimum velocity. """ mission_distance = end_event['max_distance'] mission_max_time = end_event['max_time'] mission_min_velocity = end_event['min_velocity'] # Assume that y[1] is the distance traveled distance_left = lambda t,y: mission_distance - y[1] distance_left.terminal = True time_left = lambda t,y: mission_max_time - t time_left.terminal = True velocity_threshold = lambda t,y: y[0] - mission_min_velocity; velocity_threshold.terminal = True velocity_threshold.direction = -1 # terminal indicates whether any of the conditions can lead to the # termination of the ODE solver. In this case all conditions can terminate # the simulation independently. # direction indicates whether the direction along which the different # conditions is reached matter or does not matter. In this case, only # the direction in which the velocity treshold is arrived at matters # (negative) events = [distance_left, time_left, velocity_threshold] return events def experiment1(): experiment = {'time_range' : np.array([0,20000]), 'initial_conditions' : np.array([0.3125,0]), 'alpha_dist' : np.array([0, 100, 200, 300, 400, 500, 600, 700, 800, 900, 1000]), 'alpha_deg' : np.array([11.509, 2.032, 7.182, 2.478, \ 5.511, 10.981, 5.601, -0.184, \ 0.714, 4.151, 4.042]), 'Crr' : 0.1} # Below are default values for example only: end_event = {'max_distance' : 50, 'max_time' : 5000, 'min_velocity' : 0.01} return experiment, end_event def simulate_rover(rover, planet, experiment, end_event): ''' Simulates the trajectory of a rover on a planet surface and returns updated telemetry information. Inputs: rover (dict): A dictionary containing the parameters of the rover. planet (dict): A dictionary containing the planet definition. experiment (dict): A dictionary containing parameters of the trajectory to be followed by the rover. end_event (dict): A dictionary containing the conditions necessary and sufficient to terminate simulation of rover dynamics. Returns: rover (dict): A dictionary containing the parameters of the rover, including updated telemetry information. ''' # Check inputs are dictionaries if not isinstance(experiment, dict): raise TypeError('Input must be a dictionary') if not isinstance(end_event, dict): raise TypeError('Input must be a dictionary') # Set initial conditions and differential equation to solve t_span = experiment["time_range"] y0 = experiment["initial_conditions"] function = lambda time, y: rover_dynamics(time, y, rover, planet, experiment) # Solve the differential equation and return solution solution = solve_ivp(function, t_span=t_span, y0=y0, method='RK45', events=end_of_mission_event(end_event)) # Update rover telemetry rover['telemetry']['Time'] = solution.t rover['telemetry']['completion_time'] = solution.t[-1] rover['telemetry']['velocity'] = solution.y[0] rover['telemetry']['position'] = solution.y[1] rover['telemetry']['distance_traveled'] = solution.y[1][-1] rover['telemetry']['max_velocity'] = np.max(solution.y[0]) rover['telemetry']['average_velocity'] = np.mean(solution.y[0]) rover['telemetry']['power'] = mechpower(solution.y[0], rover) rover['telemetry']['battery_energy'] = battenergy(solution.t, solution.y[0], rover) rover['telemetry']['energy_per_distance'] = rover['telemetry']['battery_energy'] / rover['telemetry']['distance_traveled'] return rover