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):
# '''
# General Description
# This function computes the instantaneous mechanical power output by a single DC motor at each point in a given
# velocity profile.
# Calling Syntax
# P = mechpower(v,rover)
# Input Arguments
# v 1D numpy array
# OR scalar float/int
# Rover velocity data obtained from a simulation [m/s]
# rover dict Data structure containing rover definition
# Return Arguments
# P 1D numpy array
# OR scalar float/int
# Instantaneous power output of a single motor corresponding to
# each element in v [W]. Return argument should be the same size as
# input v.
# Additional Specifications and Notes
# ▪ This function should validate (a) that the first input is a scalar or vector of numerical values and (b) that the
# second input is a dict. If any condition fails, call raise Exception().
# ▪ This function should call tau_dcmotor and motorW.
# ▪ Be wary of units
# '''
# '''Computes instantaneous power output by a SINGLE DC motor at each point in a given velocity profile'''
# # Validate input is a dict
# if not isinstance(rover, dict):
# raise Exception("Invalid input: rover must be a dictionary.")
# # Validate input is a scalar or a vector
# if not isinstance(v, (int, float, np.ndarray)):
# raise Exception("Invalid input: v must be a scalar or a vector.")
# elif not isinstance(v, np.ndarray):
# v = np.array([v], dtype=float) # Convert scalar to numpy array
# elif len(v.shape) != 1:
# raise Exception("Invalid input: v must be a 1D numpy array.")
# # Compute omega and torque using helper functions
# omega = motorW(v, rover)
# tau = tau_dcmotor(omega, rover["wheel_assembly"]["motor"])
# # Compute power and return
# P = omega * tau
# return P
# def battenergy(t, v, rover):
# """
# General Description
# This function computes the total electrical energy consumed from the rover battery pack over a simulation profile,
# defined as time-velocity pairs. This function assumes all 6 motors are driven from the same battery pack (i.e., this
# function accounts for energy consumed by all motors).
# This function accounts for the inefficiencies of transforming electrical energy to mechanical energy using a DC
# motor.
# In practice, the result given by this function will be a lower bound on energy requirements since it is undesirable to
# run batteries to zero capacity and other losses exist that are not modeled in this project.
# Calling Syntax
# E = battenergy(t,v,rover)
# Input Arguments
# t 1D numpy array N-element array of time samples from a rover simulation [s]
# v 1D numpy array N-element array of rover velocity data from a simulation [m/s]
# rover dict Data structure containing rover definition
# Return Arguments
# E scalar Total electrical energy consumed from the rover battery pack over
# the input simulation profile. [J]
# Additional Specifications and Notes
# ▪ This function should validate (a) that the first two inputs are equal-length vectors of numerical values and (b)
# that the third input is a dict. If any condition fails, call raise Exception().
# ▪ This function should call mechpower and tau_dcmotor.
# ▪ The rover dictionary should contain data points for efficiency as a function of torque as noted in Section 4 and
# also in Task 5. You will need to interpolate between these data points (as you did in Task 5) to evaluate the
# efficiency for a given torque.
# ▪ Be wary of units
# """
# # Validate inputs
# if type(rover) != dict or not isinstance(v, np.ndarray) or not isinstance(t, np.ndarray) or np.shape(v) != np.shape(t):
# raise Exception("Invalid inputs for battenergy function.")
# # Get motor parameters
# omega = motorW(v, rover)
# P = mechpower(v, rover)
# torque = tau_dcmotor(omega, rover['wheel_assembly']['motor'])
# effcy = interp1d(rover['wheel_assembly']['motor']['effcy_tau'], rover['wheel_assembly']['motor']['effcy'], kind='cubic')
# # Calculate battery power at each point
# P_batt = P / effcy(torque)
# # Integrate power to find total energy
# E_OneMotor = simps(P_batt, x=t)
# E = E_OneMotor * 6 # multiply by number of motors
# return E
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):
'''
This function simulates the trajectory of a rover on a planet surface.
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 validity of inputs
if not isinstance(experiment, dict):
raise TypeError("experiment must be a dictionary")
if not isinstance(end_event, dict):
raise TypeError("end_event must be a dictionary")
# Set initial conditions and integrate the differential equations
t_span = experiment["time_range"]
y0 = experiment["initial_conditions"]
function = lambda time, y: rover_dynamics(time, y, rover, planet, experiment)
solution = solve_ivp(function, t_span=t_span, y0=y0, method='RK45', events=end_of_mission_event(end_event))
# Populate the rover telemetry dictionary
telemetry = rover.setdefault('telemetry', {})
telemetry['Time'] = solution.t
telemetry['completion_time'] = solution.t[-1]
telemetry['velocity'] = solution.y[0]
telemetry['position'] = solution.y[1]
telemetry['distance_traveled'] = solution.y[1][-1]
telemetry['max_velocity'] = max(telemetry['velocity'])
telemetry['average_velocity'] = np.mean(telemetry['velocity'])
telemetry['power'] = mechpower(telemetry['velocity'], rover)
telemetry['battery_energy'] = battenergy(telemetry['Time'], telemetry['velocity'], rover)
telemetry['energy_per_distance'] = telemetry['battery_energy'] / telemetry['distance_traveled']
return rover
