Source code for sharpy.generators.trajectorygenerator

import numpy as np
from numpy.polynomial import polynomial as P
import scipy as sc
from scipy import interpolate

import sharpy.utils.generator_interface as generator_interface
import sharpy.utils.settings as settings
import sharpy.utils.cout_utils as cout


[docs]@generator_interface.generator class TrajectoryGenerator(generator_interface.BaseGenerator): """ ``TrajectoryGenerator`` is used to generate nodal positions or velocities for trajectory constraints such as the ones included in the multibody solver. It is usually called from a ``Controller`` module. """ generator_id = 'TrajectoryGenerator' generator_classification = 'utils' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['angle_end'] = 'float' settings_default['angle_end'] = 0.0 settings_description['angle_end'] = 'Trajectory angle wrt horizontal at release' settings_types['veloc_end'] = 'float' settings_default['veloc_end'] = None settings_description['veloc_end'] = 'Release velocity at release' settings_types['shape'] = 'str' settings_default['shape'] = 'quadratic' settings_description['shape'] = 'Shape of the ``z`` vs ``x`` function. ``quadratic`` or ``linear`` are supported' settings_types['acceleration'] = 'str' settings_default['acceleration'] = 'linear' settings_description['acceleration'] = 'Acceleration law, possible values are ``linear`` or ``constant``' settings_types['dt'] = 'float' settings_default['dt'] = None settings_description['dt'] = 'Time step of the simulation' settings_types['coords_end'] = 'list(float)' settings_default['coords_end'] = None settings_description['coords_end'] = 'Coordinates of the final ramp point' settings_types['plot'] = 'bool' settings_default['plot'] = False settings_description['plot'] = 'Plot the ramp shape. Requires matplotlib installed' settings_types['print_info'] = 'bool' settings_default['print_info'] = False settings_description['print_info'] = 'Print information on runtime' settings_types['time_offset'] = 'float' settings_default['time_offset'] = 0.0 settings_description['time_offset'] = 'Time interval before the start of the ramp acceleration' settings_types['offset'] = 'list(float)' settings_default['offset'] = np.zeros((3,)) settings_description['offset'] = 'Coordinates of the starting point of the simulation' settings_types['return_velocity'] = 'bool' settings_default['return_velocity'] = False settings_description['return_velocity'] = 'If ``True``, nodal velocities are given, if ``False``, coordinates are the output' settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) def __init__(self): self.in_dict = dict() self.x_vec = None self.y_vec = None self.x_dot_vec = None self.y_dot_vec = None self.time = None self.n_steps = None self.travel_time = None self.curve_length = None self.implemented_shapes = ["linear", "quadratic"] self.implemented_accelerations = ["constant", "linear"] def initialise(self, in_dict): self.in_dict = in_dict settings.to_custom_types(self.in_dict, self.settings_types, self.settings_default) # input validation self.in_dict['shape'] = self.in_dict['shape'].lower() self.in_dict['acceleration'] = self.in_dict['acceleration'].lower() self.calculate_trajectory() if self.in_dict['print_info']: self.print_info() if self.in_dict['plot']: self.plot() if self.in_dict['return_velocity']: self.diff_trajectory() def print_info(self): cout.cout_wrap('Trajectory information:', 2) cout.cout_wrap('\t-travel time: {}'.format(self.travel_time), 2) cout.cout_wrap('\t-curve length: {}'.format(self.curve_length), 2) cout.cout_wrap('-------------------------------', 2) def plot(self): try: import matplotlib.pyplot as plt plt.figure() plt.scatter(self.x_vec, self.y_vec, c=self.time_vec) plt.colorbar(orientation='horizontal') plt.axis('equal') plt.show() except ModuleNotFoundError: import warnings warnings.warn('Unable to import matplotlib, skipping plot') def get_n_steps(self): return self.n_steps def __call__(self, params): return self.generate(params) def generate(self, params): it = params['it'] if self.n_steps is not None: if it >= self.n_steps: return [None]*3 if self.in_dict['return_velocity']: return np.array([self.x_dot_vec[it], 0.0, self.y_dot_vec[it]]) + self.in_dict['offset'] else: return np.array([self.x_vec[it], 0.0, self.y_vec[it]]) + self.in_dict['offset'] def calculate_trajectory(self): in_dict = self.in_dict # shape variables shape_polynomial = np.zeros((3,)) # [c, b, a] results in y = c + b*x + a*x^2 curve_length = 0.0 # calculate coefficients and curve length if in_dict['shape'] == "linear": shape_polynomial[0] = 0.0 shape_polynomial[1] = in_dict['coords_end'][1]/in_dict['coords_end'][0] if np.isnan(shape_polynomial[1]): shape_polynomial[1] = 0.0 shape_polynomial[2] = 0.0 curve_length = linear_curve_length(shape_polynomial, in_dict['coords_end']) elif in_dict['shape'] == "quadratic": shape_polynomial[2] = (np.arctan(in_dict['angle_end'].value) - in_dict['coords_end'][1]/in_dict['coords_end'][0])/in_dict['coords_end'][0] shape_polynomial[1] = np.arctan(in_dict['angle_end'].value) - 2.0*shape_polynomial[2]*in_dict['coords_end'][0] shape_polynomial[0] = 0.0 curve_length = quadratic_curve_length(shape_polynomial, in_dict['coords_end']) curve_length = np.abs(curve_length) # acceleration acceleration_position_coefficients = None travel_time = 0.0 if in_dict['acceleration'] == 'constant': acceleration_position_coefficients = constant_acceleration_position_coeffs(curve_length, np.abs(in_dict['veloc_end'].value)) travel_time = constant_acceleration_travel_time(curve_length, np.abs(in_dict['veloc_end'].value)) elif in_dict['acceleration'] == 'linear': acceleration_position_coefficients = linear_acceleration_position_coeffs(curve_length, np.abs(in_dict['veloc_end'].value)) travel_time = linear_acceleration_travel_time(curve_length, np.abs(in_dict['veloc_end'].value)) # time n_steps_travel = int(round(travel_time/in_dict['dt'].value)) n_steps_offset = int(round(self.in_dict['time_offset'].value/in_dict['dt'].value)) self.n_steps = n_steps_offset + n_steps_travel self.time_vec = np.zeros((self.n_steps, )) self.time_vec[n_steps_offset:] = np.linspace(0.0, n_steps_travel*in_dict['dt'].value, n_steps_travel) # with t I get s s_vec = P.polyval(self.time_vec, acceleration_position_coefficients) # need to get a function for x(s) # sample s(x) and create an interpolator n_samples = 1000 sampled_x_vec = np.linspace(0.0, in_dict['coords_end'][0], n_samples) sampled_s_vec = np.zeros((n_samples, )) for i_sample in range(n_samples): if in_dict['shape'] == "linear": sampled_s_vec[i_sample] = np.abs(linear_curve_length(shape_polynomial, np.array([sampled_x_vec[i_sample], 0.0]))) elif in_dict['shape'] == 'quadratic': sampled_s_vec[i_sample] = np.abs(quadratic_curve_length(shape_polynomial, np.array([sampled_x_vec[i_sample], 0.0]))) x_of_s_interp = sc.interpolate.interp1d(sampled_s_vec, sampled_x_vec, kind='quadratic', fill_value='extrapolate') self.x_vec = x_of_s_interp(s_vec) # with x, I obtain y and done self.y_vec = P.polyval(self.x_vec, shape_polynomial) self.travel_time = travel_time self.curve_length = curve_length def diff_trajectory(self): dt = self.in_dict['dt'].value self.x_dot_vec = np.gradient(self.x_vec, dt) self.y_dot_vec = np.gradient(self.y_vec, dt)
def linear_curve_length(shape_polynomial, coords_end): dzdx_end = shape_polynomial[1] length = coords_end[0]*np.sqrt(dzdx_end**2 + 1) return length def quadratic_curve_length(shape_polynomial, coords_end): dzdx_end = 2.0*shape_polynomial[2]*coords_end[0] + shape_polynomial[1] a = shape_polynomial[2] b = shape_polynomial[1] xe = coords_end[0] length = (2.0*a*xe + b)*np.sqrt((2.0*a*xe + b)**2 + 1.0) length += np.arcsinh(2.0*a*xe + b) length -= b*np.sqrt(b**2 + 1.0) length -= np.arcsinh(b) length /= 4.0*a return length def constant_acceleration_position_coeffs(s_e, s_dot_e): coeffs = (0.0, 0.0, (4.0*s_dot_e**2)/s_e, 0.0) return coeffs def linear_acceleration_position_coeffs(s_e, s_dot_e): coeff = ((6.0*s_e)**(2./3.)/(2.0*s_dot_e))**(-3.) coeffs = (0.0, 0.0, 0.0, (1./6.)*coeff) return coeffs def constant_acceleration_travel_time(s_e, s_dot_e): return 2.0*s_e/s_dot_e def linear_acceleration_travel_time(s_e, s_dot_e): return 3.0*s_e/s_dot_e