def yeadon_vec_to_bicycle_vec(vector, measured_bicycle_par, benchmark_bicycle_par): """ Parameters ---------- vector : np.matrix, shape(3, 1) A vector from the Yeadon origin to a point expressed in the Yeadon reference frame. measured_bicycle_par : dictionary The raw bicycle measurements. benchmark_bicycle_par : dictionary The Meijaard 2007 et. al parameters for this bicycle. Returns ------- vector_wrt_bike : np.matrix, shape(3, 1) The vector from the bicycle origin to the same point above expressed in the bicycle reference frame. """ # This is the rotation matrix that relates Yeadon's reference frame # to the bicycle reference frame. # vector_expressed_in_bike = rot_mat * vector_expressed_in_yeadon) rot_mat = np.matrix([[0.0, -1.0, 0.0], [-1.0, 0.0, 0.0], [0.0, 0.0, -1.0]]) # The relevant bicycle parameters: measuredPar = remove_uncertainties(measured_bicycle_par) benchmarkPar = remove_uncertainties(benchmark_bicycle_par) # bottom bracket height hbb = measuredPar['hbb'] # chain stay length lcs = measuredPar['lcs'] # rear wheel radius rR = benchmarkPar['rR'] # seat post length lsp = measuredPar['lsp'] # seat tube length lst = measuredPar['lst'] # seat tube angle lambdast = measuredPar['lamst'] # bicycle origin to yeadon origin expressed in bicycle frame yeadon_origin_in_bike_frame = \ np.matrix([[np.sqrt(lcs**2 - (-hbb + rR)**2) + (-lsp - lst) * np.cos(lambdast)], # bx [0.0], [-hbb + (-lsp - lst) * np.sin(lambdast)]]) # bz vector_wrt_bike = yeadon_origin_in_bike_frame + rot_mat * vector return vector_wrt_bike
def eig(self, speeds): '''Returns the eigenvalues and eigenvectors of the Whipple bicycle model linearized about the nominal configuration. Parameters ---------- speeds : ndarray, shape (n,) or float The speed at which to calculate the eigenvalues. Returns ------- evals : ndarray, shape (n, 4) eigenvalues evecs : ndarray, shape (n, 4, 4) eigenvectors Notes ----- If you have a flywheel defined, body D, it will completely be ignored in these results. These results are strictly for the Whipple bicycle model. ''' # this allows you to enter a float try: speeds.shape except AttributeError: speeds = np.array([speeds]) par = io.remove_uncertainties(self.parameters['Benchmark']) M, C1, K0, K2 = bicycle.benchmark_par_to_canonical(par) m, n = 4, speeds.shape[0] evals = np.zeros((n, m), dtype='complex128') evecs = np.zeros((n, m, m), dtype='complex128') for i, speed in enumerate(speeds): A, B = bicycle.ab_matrix(M, C1, K0, K2, speed, par['g']) w, v = np.linalg.eig(A) evals[i] = w evecs[i] = v return evals, evecs
def plot_bicycle_geometry(self, show=True, pendulum=True, centerOfMass=True, inertiaEllipse=True): '''Returns a figure showing the basic bicycle geometry, the centers of mass and the moments of inertia. Notes ----- If the flywheel is defined, it's center of mass corresponds to the front wheel and is not depicted in the plot. ''' par = io.remove_uncertainties(self.parameters['Benchmark']) parts = get_parts_in_parameters(par) try: slopes = io.remove_uncertainties(self.extras['slopes']) intercepts = io.remove_uncertainties(self.extras['intercepts']) penInertias = io.remove_uncertainties( self.extras['pendulumInertias']) except AttributeError: pendulum = False fig = plt.figure() ax = plt.axes() # define some colors for the parts numColors = len(parts) cmap = plt.get_cmap('gist_rainbow') partColors = {} for i, part in enumerate(parts): partColors[part] = cmap(1. * i / numColors) if inertiaEllipse: # plot the principal moments of inertia for j, part in enumerate(parts): I = inertia.part_inertia_tensor(par, part) Ip, C = inertia.principal_axes(I) if part == 'R': center = np.array([0., par['rR']]) elif part in 'FD': center = np.array([par['w'], par['rF']]) else: center = np.array([par['x' + part], -par['z' + part]]) # which row in C is the y vector uy = np.array([0., 1., 0.]) for i, row in enumerate(C): if np.abs(np.sum(row - uy)) < 1E-10: yrow = i # remove the row for the y vector Ip2D = np.delete(Ip, yrow, 0) # remove the column and row associated with the y C2D = np.delete(np.delete(C, yrow, 0), 1, 1) # make an ellipse Imin = Ip2D[0] Imax = Ip2D[1] # get width and height of a ellipse with the major axis equal # to one unitWidth = 1. / 2. / np.sqrt(Imin) * np.sqrt(Imin) unitHeight = 1. / 2. / np.sqrt(Imax) * np.sqrt(Imin) # now scaled the width and height relative to the maximum # principal moment of inertia width = Imax * unitWidth height = Imax * unitHeight angle = -np.degrees(np.arccos(C2D[0, 0])) ellipse = Ellipse((center[0], center[1]), width, height, angle=angle, fill=False, color=partColors[part], alpha=0.25) ax.add_patch(ellipse) # plot the ground line x = np.array([-par['rR'], par['w'] + par['rF']]) plt.plot(x, np.zeros_like(x), 'k') # plot the rear wheel c = plt.Circle((0., par['rR']), radius=par['rR'], fill=False) ax.add_patch(c) # plot the front wheel c = plt.Circle((par['w'], par['rF']), radius=par['rF'], fill=False) ax.add_patch(c) # plot the fundamental bike deex, deez = geometry.fundamental_geometry_plot_data(par) plt.plot(deex, -deez, 'k') # plot the steer axis dx3 = deex[2] + deez[2] * (deex[2] - deex[1]) / (deez[1] - deez[2]) plt.plot([deex[2], dx3], [-deez[2], 0.], 'k--') # don't plot the pendulum lines if a rider has been added because the # inertia has changed if self.hasRider: pendulum = False if pendulum: # plot the pendulum axes for the measured parts for j, pair in enumerate(slopes.items()): part, slopeSet = pair xcom, zcom = par['x' + part], par['z' + part] for i, m in enumerate(slopeSet): b = intercepts[part][i] xPoint, zPoint = geometry.project_point_on_line( (m, b), (xcom, zcom)) comLineLength = penInertias[part][i] xPlus = comLineLength / 2. * np.cos(np.arctan(m)) x = np.array([xPoint - xPlus, xPoint + xPlus]) z = -m * x - b plt.plot(x, z, color=partColors[part]) # label the pendulum lines with a number plt.text(x[0], z[0], str(i + 1)) if centerOfMass: # plot the center of mass location def com_symbol(ax, center, radius, color='b'): '''Returns axis with center of mass symbol.''' c = plt.Circle(center, radius=radius, fill=False) w1 = Wedge(center, radius, 0., 90., color=color, ec=None, alpha=0.5) w2 = Wedge(center, radius, 180., 270., color=color, ec=None, alpha=0.5) ax.add_patch(w1) ax.add_patch(w2) ax.add_patch(c) return ax # radius of the CoM symbol sRad = 0.03 # front wheel CoM ax = com_symbol(ax, (par['w'], par['rF']), sRad, color=partColors['F']) plt.text(par['w'] + sRad, par['rF'] + sRad, 'F') # rear wheel CoM ax = com_symbol(ax, (0., par['rR']), sRad, color=partColors['R']) plt.text(0. + sRad, par['rR'] + sRad, 'R') for j, part in enumerate([x for x in parts if x not in 'RFD']): xcom = par['x' + part] zcom = par['z' + part] ax = com_symbol(ax, (xcom, -zcom), sRad, color=partColors[part]) plt.text(xcom + sRad, -zcom + sRad, part) if 'H' not in parts: ax = com_symbol(ax, (par['xH'], -par['zH']), sRad) plt.text(par['xH'] + sRad, -par['zH'] + sRad, 'H') plt.axis('equal') plt.ylim((0., 1.)) plt.title(self.bicycleName) # if there is a rider on the bike, make a simple stick figure if self.human: human = self.human mpar = self.parameters['Measured'] bpar = self.parameters['Benchmark'] # K2: lower leg, tip of foot to knee start = rider.yeadon_vec_to_bicycle_vec(human.K2.end_pos, mpar, bpar) end = rider.yeadon_vec_to_bicycle_vec(human.K2.pos, mpar, bpar) plt.plot([start[0, 0], end[0, 0]], [-start[2, 0], -end[2, 0]], 'k') # K1: upper leg, knee to hip start = rider.yeadon_vec_to_bicycle_vec(human.K2.pos, mpar, bpar) end = rider.yeadon_vec_to_bicycle_vec(human.K1.pos, mpar, bpar) plt.plot([start[0, 0], end[0, 0]], [-start[2, 0], -end[2, 0]], 'k') # torso start = rider.yeadon_vec_to_bicycle_vec(human.K1.pos, mpar, bpar) end = rider.yeadon_vec_to_bicycle_vec(human.B1.pos, mpar, bpar) plt.plot([start[0, 0], end[0, 0]], [-start[2, 0], -end[2, 0]], 'k') # B1: upper arm start = rider.yeadon_vec_to_bicycle_vec(human.B1.pos, mpar, bpar) end = rider.yeadon_vec_to_bicycle_vec(human.B2.pos, mpar, bpar) plt.plot([start[0, 0], end[0, 0]], [-start[2, 0], -end[2, 0]], 'k') # B2: lower arm, elbow to tip of fingers start = rider.yeadon_vec_to_bicycle_vec(human.B2.pos, mpar, bpar) end = rider.yeadon_vec_to_bicycle_vec(human.B2.end_pos, mpar, bpar) plt.plot([start[0, 0], end[0, 0]], [-start[2, 0], -end[2, 0]], 'k') # C: chest/head start = rider.yeadon_vec_to_bicycle_vec(human.B1.pos, mpar, bpar) end = rider.yeadon_vec_to_bicycle_vec(human.C.end_pos, mpar, bpar) plt.plot([start[0, 0], end[0, 0]], [-start[2, 0], -end[2, 0]], 'k') if show: fig.show() return fig
def rider_on_bike(benchmarkPar, measuredPar, yeadonMeas, yeadonCFG, drawrider): """ Returns a yeadon human configured to sit on a bicycle. Parameters ---------- benchmarkPar : dictionary A dictionary containing the benchmark bicycle parameters. measuredPar : dictionary A dictionary containing the raw geometric measurements of the bicycle. yeadonMeas : str Path to a text file that holds the 95 yeadon measurements. See `yeadon documentation`_. yeadonCFG : str Path to a text file that holds configuration variables. See `yeadon documentation`_. As of now, only 'somersalt' angle can be set as an input. The remaining variables are either zero or calculated in this method. drawrider : bool Switch to draw the rider, with vectors pointing to the desired position of the hands and feet of the rider (at the handles and bottom bracket). Requires python-visual. Returns ------- human : yeadon.Human Human object is returned with an updated configuration. The dictionary, taken from H.CFG, has the following key's values updated: 'PJ1extension' 'J1J2flexion' 'CA1extension' 'CA1adduction' 'CA1rotation' 'A1A2extension' 'somersault' 'PK1extension' 'K1K2flexion' 'CB1extension' 'CB1abduction' 'CB1rotation' 'B1B2extension' Notes ----- Requires that the bike object has a raw data text input file that contains the measurements necessary to situate a rider on the bike (i.e. ``<pathToData>/bicycles/<short name>/RawData/<short name>Measurements.txt``). .. _yeadon documentation : http://packages.python.org/yeadon """ # create human using input measurements and configuration files human = yeadon.Human(yeadonMeas, yeadonCFG) # The relevant human measurments: L_j3L = human.meas['Lj3L'] L_j5L = human.meas['Lj5L'] L_j6L = human.meas['Lj6L'] L_s4L = human.meas['Ls4L'] L_s4w = human.meas['Ls4w'] L_a2L = human.meas['La2L'] L_a4L = human.meas['La4L'] L_a5L = human.meas['La5L'] somersault = human.CFG['somersault'] # The relevant bicycle parameters: measuredPar = remove_uncertainties(measuredPar) benchmarkPar = remove_uncertainties(benchmarkPar) # bottom bracket height h_bb = measuredPar['hbb'] # chain stay length l_cs = measuredPar['lcs'] # rear wheel radius r_R = benchmarkPar['rR'] # front wheel radius r_F = benchmarkPar['rF'] # seat post length l_sp = measuredPar['lsp'] # seat tube length l_st = measuredPar['lst'] # seat tube angle lambda_st = measuredPar['lamst'] # handlebar width w_hb = measuredPar['whb'] # distance from rear wheel hub to hand L_hbR = measuredPar['LhbR'] # distance from front wheel hub to hand L_hbF = measuredPar['LhbF'] # wheelbase w = benchmarkPar['w'] def zero(unknowns): """For the derivation of these equations see: http://nbviewer.ipython.org/github/chrisdembia/yeadon/blob/v1.2.0/examples/bicyclerider/bicycle_example.ipynb """ PJ1extension = unknowns[0] J1J2flexion = unknowns[1] CA1extension = unknowns[2] CA1adduction = unknowns[3] CA1rotation = unknowns[4] A1A2extension = unknowns[5] alpha_y = unknowns[6] alpha_z = unknowns[7] beta_y = unknowns[8] beta_z = unknowns[9] phi_J1 = PJ1extension phi_J2 = J1J2flexion phi_A1 = CA1extension theta_A1 = CA1adduction psi_A = CA1rotation phi_A2 = A1A2extension phi_P = somersault zero = np.zeros(10) zero[0] = (L_j3L * (-sin(phi_J1) * cos(phi_P) - sin(phi_P) * cos(phi_J1)) + (-l_sp - l_st) * cos(lambda_st) + (-(-sin(phi_J1) * sin(phi_P) + cos(phi_J1) * cos(phi_P)) * sin(phi_J2) + (-sin(phi_J1) * cos(phi_P) - sin(phi_P) * cos(phi_J1)) * cos(phi_J2)) * (-L_j3L + L_j5L + L_j6L)) zero[1] = (L_j3L * (-sin(phi_J1) * sin(phi_P) + cos(phi_J1) * cos(phi_P)) + (-l_sp - l_st) * sin(lambda_st) + ((-sin(phi_J1) * sin(phi_P) + cos(phi_J1) * cos(phi_P)) * cos(phi_J2) - (sin(phi_J1) * cos(phi_P) + sin(phi_P) * cos(phi_J1)) * sin(phi_J2)) * (-L_j3L + L_j5L + L_j6L)) zero[2] = -L_hbF + sqrt(alpha_y**2 + alpha_z**2 + 0.25 * w_hb**2) zero[3] = -L_hbR + sqrt(beta_y**2 + beta_z**2 + 0.25 * w_hb**2) zero[4] = alpha_y - beta_y - w zero[5] = alpha_z - beta_z + r_F - r_R zero[6] = (-L_a2L * sin(theta_A1) + L_s4w / 2 - 0.5 * w_hb + (sin(phi_A2) * sin(psi_A) * cos(theta_A1) + sin(theta_A1) * cos(phi_A2)) * (L_a2L - L_a4L - L_a5L)) zero[7] = ( -L_a2L * (-sin(phi_A1) * cos(phi_P) * cos(theta_A1) - sin(phi_P) * cos(phi_A1) * cos(theta_A1)) - L_s4L * sin(phi_P) - beta_y - sqrt(l_cs**2 - (-h_bb + r_R)**2) - (-l_sp - l_st) * cos(lambda_st) + (-(-(sin(phi_A1) * cos(psi_A) + sin(psi_A) * sin(theta_A1) * cos(phi_A1)) * sin(phi_P) + (-sin(phi_A1) * sin(psi_A) * sin(theta_A1) + cos(phi_A1) * cos(psi_A)) * cos(phi_P)) * sin(phi_A2) + (-sin(phi_A1) * cos(phi_P) * cos(theta_A1) - sin(phi_P) * cos(phi_A1) * cos(theta_A1)) * cos(phi_A2)) * (L_a2L - L_a4L - L_a5L)) zero[8] = (-L_a2L * (-sin(phi_A1) * sin(phi_P) * cos(theta_A1) + cos(phi_A1) * cos(phi_P) * cos(theta_A1)) + L_s4L * cos(phi_P) - beta_z + h_bb - r_R - (-l_sp - l_st) * sin(lambda_st) + (-((sin(phi_A1) * cos(psi_A) + sin(psi_A) * sin(theta_A1) * cos(phi_A1)) * cos(phi_P) + (-sin(phi_A1) * sin(psi_A) * sin(theta_A1) + cos(phi_A1) * cos(psi_A)) * sin(phi_P)) * sin(phi_A2) + (-sin(phi_A1) * sin(phi_P) * cos(theta_A1) + cos(phi_A1) * cos(phi_P) * cos(theta_A1)) * cos(phi_A2)) * (L_a2L - L_a4L - L_a5L)) zero[9] = ((sin(phi_A1) * sin(psi_A) - sin(theta_A1) * cos(phi_A1) * cos(psi_A)) * cos(phi_P) + (sin(phi_A1) * sin(theta_A1) * cos(psi_A) + sin(psi_A) * cos(phi_A1)) * sin(phi_P)) return zero g_PJ1extension = -np.deg2rad(90.0) g_J1J2flexion = np.deg2rad(75.0) g_CA1extension = -np.deg2rad(15.0) g_CA1adduction = np.deg2rad(2.0) g_CA1rotation = np.deg2rad(2.0) g_A1A2extension = -np.deg2rad(40.0) g_alpha_y = L_hbF * np.cos(np.deg2rad(45.0)) g_alpha_z = L_hbF * np.sin(np.deg2rad(45.0)) g_beta_y = -L_hbR * np.cos(np.deg2rad(30.0)) g_beta_z = L_hbR * np.sin(np.deg2rad(30.0)) guess = [ g_PJ1extension, g_J1J2flexion, g_CA1extension, g_CA1adduction, g_CA1rotation, g_A1A2extension, g_alpha_y, g_alpha_z, g_beta_y, g_beta_z ] solution = fsolve(zero, guess) cfg_dict = human.CFG.copy() cfg_dict['PJ1extension'] = solution[0] cfg_dict['J1J2flexion'] = solution[1] cfg_dict['CA1extension'] = solution[2] cfg_dict['CA1adduction'] = solution[3] cfg_dict['CA1rotation'] = solution[4] cfg_dict['A1A2extension'] = solution[5] cfg_dict['somersault'] = somersault cfg_dict['PK1extension'] = cfg_dict['PJ1extension'] cfg_dict['K1K2flexion'] = cfg_dict['J1J2flexion'] cfg_dict['CB1extension'] = cfg_dict['CA1extension'] cfg_dict['CB1abduction'] = -cfg_dict['CA1adduction'] cfg_dict['CB1rotation'] = -cfg_dict['CA1rotation'] cfg_dict['B1B2extension'] = cfg_dict['A1A2extension'] # assign configuration to human and check that the solution worked human.set_CFG_dict(cfg_dict) # draw rider for fun, but possibly to check results aren't crazy if drawrider: human.draw() return human
def rider_on_bike(benchmarkPar, measuredPar, yeadonMeas, yeadonCFG, drawrider): """ Returns a yeadon human configured to sit on a bicycle. Parameters ---------- benchmarkPar : dictionary A dictionary containing the benchmark bicycle parameters. measuredPar : dictionary A dictionary containing the raw geometric measurements of the bicycle. yeadonMeas : str Path to a text file that holds the 95 yeadon measurements. See `yeadon documentation`_. yeadonCFG : str Path to a text file that holds configuration variables. See `yeadon documentation`_. As of now, only 'somersalt' angle can be set as an input. The remaining variables are either zero or calculated in this method. drawrider : bool Switch to draw the rider, with vectors pointing to the desired position of the hands and feet of the rider (at the handles and bottom bracket). Requires python-visual. Returns ------- human : yeadon.Human Human object is returned with an updated configuration. The dictionary, taken from H.CFG, has the following key's values updated: 'PJ1extension' 'J1J2flexion' 'CA1extension' 'CA1adduction' 'CA1rotation' 'A1A2extension' 'somersault' 'PK1extension' 'K1K2flexion' 'CB1extension' 'CB1abduction' 'CB1rotation' 'B1B2extension' Notes ----- Requires that the bike object has a raw data text input file that contains the measurements necessary to situate a rider on the bike (i.e. ``<pathToData>/bicycles/<short name>/RawData/<short name>Measurements.txt``). .. _yeadon documentation : http://packages.python.org/yeadon """ # create human using input measurements and configuration files human = yeadon.Human(yeadonMeas, yeadonCFG) # The relevant human measurments: L_j3L = human.meas['Lj3L'] L_j5L = human.meas['Lj5L'] L_j6L = human.meas['Lj6L'] L_s4L = human.meas['Ls4L'] L_s4w = human.meas['Ls4w'] L_a2L = human.meas['La2L'] L_a4L = human.meas['La4L'] L_a5L = human.meas['La5L'] somersault = human.CFG['somersault'] # The relevant bicycle parameters: measuredPar = remove_uncertainties(measuredPar) benchmarkPar = remove_uncertainties(benchmarkPar) # bottom bracket height h_bb = measuredPar['hbb'] # chain stay length l_cs = measuredPar['lcs'] # rear wheel radius r_R = benchmarkPar['rR'] # front wheel radius r_F = benchmarkPar['rF'] # seat post length l_sp = measuredPar['lsp'] # seat tube length l_st = measuredPar['lst'] # seat tube angle lambda_st = measuredPar['lamst'] # handlebar width w_hb = measuredPar['whb'] # distance from rear wheel hub to hand L_hbR = measuredPar['LhbR'] # distance from front wheel hub to hand L_hbF = measuredPar['LhbF'] # wheelbase w = benchmarkPar['w'] def zero(unknowns): """For the derivation of these equations see: http://nbviewer.ipython.org/github/chrisdembia/yeadon/blob/v1.2.0/examples/bicyclerider/bicycle_example.ipynb """ PJ1extension = unknowns[0] J1J2flexion = unknowns[1] CA1extension = unknowns[2] CA1adduction = unknowns[3] CA1rotation = unknowns[4] A1A2extension = unknowns[5] alpha_y = unknowns[6] alpha_z = unknowns[7] beta_y = unknowns[8] beta_z = unknowns[9] phi_J1 = PJ1extension phi_J2 = J1J2flexion phi_A1 = CA1extension theta_A1 = CA1adduction psi_A = CA1rotation phi_A2 = A1A2extension phi_P = somersault zero = np.zeros(10) zero[0] = (L_j3L*(-sin(phi_J1)*cos(phi_P) - sin(phi_P)*cos(phi_J1)) + (-l_sp - l_st)*cos(lambda_st) + (-(-sin(phi_J1)* sin(phi_P) + cos(phi_J1)*cos(phi_P))*sin(phi_J2) + (-sin(phi_J1)*cos(phi_P) - sin(phi_P)*cos(phi_J1))* cos(phi_J2))*(-L_j3L + L_j5L + L_j6L)) zero[1] = (L_j3L*(-sin(phi_J1)*sin(phi_P) + cos(phi_J1)*cos(phi_P)) + (-l_sp - l_st)*sin(lambda_st) + ((-sin(phi_J1)* sin(phi_P) + cos(phi_J1)*cos(phi_P))*cos(phi_J2) - (sin(phi_J1)*cos(phi_P) + sin(phi_P)*cos(phi_J1))* sin(phi_J2))*(-L_j3L + L_j5L + L_j6L)) zero[2] = -L_hbF + sqrt(alpha_y**2 + alpha_z**2 + 0.25*w_hb**2) zero[3] = -L_hbR + sqrt(beta_y**2 + beta_z**2 + 0.25*w_hb**2) zero[4] = alpha_y - beta_y - w zero[5] = alpha_z - beta_z + r_F - r_R zero[6] = (-L_a2L*sin(theta_A1) + L_s4w/2 - 0.5*w_hb + (sin(phi_A2)* sin(psi_A)*cos(theta_A1) + sin(theta_A1)*cos(phi_A2))* (L_a2L - L_a4L - L_a5L)) zero[7] = (-L_a2L*(-sin(phi_A1)*cos(phi_P)*cos(theta_A1) - sin(phi_P)*cos(phi_A1)*cos(theta_A1)) - L_s4L*sin(phi_P) - beta_y - sqrt(l_cs**2 - (-h_bb + r_R)**2) - (-l_sp - l_st)*cos(lambda_st) + (-(-(sin(phi_A1)*cos(psi_A) + sin(psi_A)*sin(theta_A1)*cos(phi_A1))*sin(phi_P) + (-sin(phi_A1)*sin(psi_A)*sin(theta_A1) + cos(phi_A1)* cos(psi_A))*cos(phi_P))*sin(phi_A2) + (-sin(phi_A1)* cos(phi_P)*cos(theta_A1) - sin(phi_P)*cos(phi_A1)* cos(theta_A1))*cos(phi_A2))*(L_a2L - L_a4L - L_a5L)) zero[8] = (-L_a2L*(-sin(phi_A1)*sin(phi_P)*cos(theta_A1) + cos(phi_A1)*cos(phi_P)*cos(theta_A1)) + L_s4L*cos(phi_P) - beta_z + h_bb - r_R - (-l_sp - l_st)*sin(lambda_st) + (-((sin(phi_A1)*cos(psi_A) + sin(psi_A)*sin(theta_A1)* cos(phi_A1))*cos(phi_P) + (-sin(phi_A1)*sin(psi_A)* sin(theta_A1) + cos(phi_A1)*cos(psi_A))*sin(phi_P))* sin(phi_A2) + (-sin(phi_A1)*sin(phi_P)*cos(theta_A1) + cos(phi_A1)*cos(phi_P)*cos(theta_A1))*cos(phi_A2))*(L_a2L - L_a4L - L_a5L)) zero[9] = ((sin(phi_A1)*sin(psi_A) - sin(theta_A1)*cos(phi_A1)* cos(psi_A))*cos(phi_P) + (sin(phi_A1)*sin(theta_A1)* cos(psi_A) + sin(psi_A)*cos(phi_A1))*sin(phi_P)) return zero g_PJ1extension = -np.deg2rad(90.0) g_J1J2flexion = np.deg2rad(75.0) g_CA1extension = -np.deg2rad(15.0) g_CA1adduction = np.deg2rad(2.0) g_CA1rotation = np.deg2rad(2.0) g_A1A2extension = -np.deg2rad(40.0) g_alpha_y = L_hbF * np.cos(np.deg2rad(45.0)) g_alpha_z = L_hbF * np.sin(np.deg2rad(45.0)) g_beta_y = -L_hbR * np.cos(np.deg2rad(30.0)) g_beta_z = L_hbR * np.sin(np.deg2rad(30.0)) guess = [g_PJ1extension, g_J1J2flexion, g_CA1extension, g_CA1adduction, g_CA1rotation, g_A1A2extension, g_alpha_y, g_alpha_z, g_beta_y, g_beta_z] solution = fsolve(zero, guess) cfg_dict = human.CFG.copy() cfg_dict['PJ1extension'] = solution[0] cfg_dict['J1J2flexion'] = solution[1] cfg_dict['CA1extension'] = solution[2] cfg_dict['CA1adduction'] = solution[3] cfg_dict['CA1rotation'] = solution[4] cfg_dict['A1A2extension'] = solution[5] cfg_dict['somersault'] = somersault cfg_dict['PK1extension'] = cfg_dict['PJ1extension'] cfg_dict['K1K2flexion'] = cfg_dict['J1J2flexion'] cfg_dict['CB1extension'] = cfg_dict['CA1extension'] cfg_dict['CB1abduction'] = -cfg_dict['CA1adduction'] cfg_dict['CB1rotation'] = -cfg_dict['CA1rotation'] cfg_dict['B1B2extension'] = cfg_dict['A1A2extension'] # assign configuration to human and check that the solution worked human.set_CFG_dict(cfg_dict) # draw rider for fun, but possibly to check results aren't crazy if drawrider: human.draw() return human
def plot_bicycle_geometry(self, show=True, pendulum=True, centerOfMass=True, inertiaEllipse=True): '''Returns a figure showing the basic bicycle geometry, the centers of mass and the moments of inertia. Notes ----- If the flywheel is defined, it's center of mass corresponds to the front wheel and is not depicted in the plot. ''' par = io.remove_uncertainties(self.parameters['Benchmark']) parts = get_parts_in_parameters(par) try: slopes = io.remove_uncertainties(self.extras['slopes']) intercepts = io.remove_uncertainties(self.extras['intercepts']) penInertias = io.remove_uncertainties(self.extras['pendulumInertias']) except AttributeError: pendulum = False fig = plt.figure() ax = plt.axes() # define some colors for the parts numColors = len(parts) cmap = plt.get_cmap('gist_rainbow') partColors = {} for i, part in enumerate(parts): partColors[part] = cmap(1. * i / numColors) if inertiaEllipse: # plot the principal moments of inertia for j, part in enumerate(parts): I = inertia.part_inertia_tensor(par, part) Ip, C = inertia.principal_axes(I) if part == 'R': center = np.array([0., par['rR']]) elif part in 'FD': center = np.array([par['w'], par['rF']]) else: center = np.array([par['x' + part], -par['z' + part]]) # which row in C is the y vector uy = np.array([0., 1., 0.]) for i, row in enumerate(C): if np.abs(np.sum(row - uy)) < 1E-10: yrow = i # remove the row for the y vector Ip2D = np.delete(Ip, yrow, 0) # remove the column and row associated with the y C2D = np.delete(np.delete(C, yrow, 0), 1, 1) # make an ellipse Imin = Ip2D[0] Imax = Ip2D[1] # get width and height of a ellipse with the major axis equal # to one unitWidth = 1. / 2. / np.sqrt(Imin) * np.sqrt(Imin) unitHeight = 1. / 2. / np.sqrt(Imax) * np.sqrt(Imin) # now scaled the width and height relative to the maximum # principal moment of inertia width = Imax * unitWidth height = Imax * unitHeight angle = -np.degrees(np.arccos(C2D[0, 0])) ellipse = Ellipse((center[0], center[1]), width, height, angle=angle, fill=False, color=partColors[part], alpha=0.25) ax.add_patch(ellipse) # plot the ground line x = np.array([-par['rR'], par['w'] + par['rF']]) plt.plot(x, np.zeros_like(x), 'k') # plot the rear wheel c = plt.Circle((0., par['rR']), radius=par['rR'], fill=False) ax.add_patch(c) # plot the front wheel c = plt.Circle((par['w'], par['rF']), radius=par['rF'], fill=False) ax.add_patch(c) # plot the fundamental bike deex, deez = geometry.fundamental_geometry_plot_data(par) plt.plot(deex, -deez, 'k') # plot the steer axis dx3 = deex[2] + deez[2] * (deex[2] - deex[1]) / (deez[1] - deez[2]) plt.plot([deex[2], dx3], [-deez[2], 0.], 'k--') # don't plot the pendulum lines if a rider has been added because the # inertia has changed if self.hasRider: pendulum = False if pendulum: # plot the pendulum axes for the measured parts for j, pair in enumerate(slopes.items()): part, slopeSet = pair xcom, zcom = par['x' + part], par['z' + part] for i, m in enumerate(slopeSet): b = intercepts[part][i] xPoint, zPoint = geometry.project_point_on_line((m, b), (xcom, zcom)) comLineLength = penInertias[part][i] xPlus = comLineLength / 2. * np.cos(np.arctan(m)) x = np.array([xPoint - xPlus, xPoint + xPlus]) z = -m * x - b plt.plot(x, z, color=partColors[part]) # label the pendulum lines with a number plt.text(x[0], z[0], str(i + 1)) if centerOfMass: # plot the center of mass location def com_symbol(ax, center, radius, color='b'): '''Returns axis with center of mass symbol.''' c = plt.Circle(center, radius=radius, fill=False) w1 = Wedge(center, radius, 0., 90., color=color, ec=None, alpha=0.5) w2 = Wedge(center, radius, 180., 270., color=color, ec=None, alpha=0.5) ax.add_patch(w1) ax.add_patch(w2) ax.add_patch(c) return ax # radius of the CoM symbol sRad = 0.03 # front wheel CoM ax = com_symbol(ax, (par['w'], par['rF']), sRad, color=partColors['F']) plt.text(par['w'] + sRad, par['rF'] + sRad, 'F') # rear wheel CoM ax = com_symbol(ax, (0., par['rR']), sRad, color=partColors['R']) plt.text(0. + sRad, par['rR'] + sRad, 'R') for j, part in enumerate([x for x in parts if x not in 'RFD']): xcom = par['x' + part] zcom = par['z' + part] ax = com_symbol(ax, (xcom, -zcom), sRad, color=partColors[part]) plt.text(xcom + sRad, -zcom + sRad, part) if 'H' not in parts: ax = com_symbol(ax, (par['xH'], -par['zH']), sRad) plt.text(par['xH'] + sRad, -par['zH'] + sRad, 'H') plt.axis('equal') plt.ylim((0., 1.)) plt.title(self.bicycleName) # if there is a rider on the bike, make a simple stick figure if self.human: human = self.human # K2: lower leg plt.plot([human.k[7].pos[0, 0], human.K2.pos[0, 0]], [-human.k[7].endpos[2, 0], -human.K2.pos[2, 0]], 'k') # K1: upper leg plt.plot([human.K2.pos[0, 0], human.K1.pos[0, 0]], [-human.K2.pos[2, 0], -human.K1.pos[2, 0]], 'k') # torso plt.plot([human.K1.pos[0, 0], human.B1.pos[0, 0]], [-human.K1.pos[2, 0], -human.B1.pos[2, 0]], 'k') # B1: upper arm plt.plot([human.B1.pos[0, 0], human.B2.pos[0, 0]], [-human.B1.pos[2, 0], -human.B2.pos[2, 0]], 'k') # B2: lower arm plt.plot([human.B2.pos[0, 0], human.b[6].pos[0, 0]], [-human.B2.pos[2, 0], -human.b[6].endpos[2, 0]], 'k') # C: chest/head plt.plot([human.B1.pos[0, 0], human.C.endpos[0, 0]], [-human.B1.pos[2, 0], -human.C.endpos[2, 0]], 'k') if show: fig.show() return fig
def rider_on_bike(benchmarkPar, measuredPar, yeadonMeas, yeadonCFG, drawrider): """ Returns a yeadon human configured to sit on a bicycle. Parameters ---------- benchmarkPar : dictionary A dictionary containing the benchmark bicycle parameters. measuredPar : dictionary A dictionary containing the raw geometric measurements of the bicycle. yeadonMeas : str Path to a text file that holds the 95 yeadon measurements. See `yeadon documentation`_. yeadonCFG : str Path to a text file that holds configuration variables. See `yeadon documentation`_. As of now, only 'somersalt' angle can be set as an input. The remaining variables are either zero or calculated in this method. drawrider : bool Switch to draw the rider, with vectors pointing to the desired position of the hands and feet of the rider (at the handles and bottom bracket). Requires python-visual. Returns ------- H : yeadon.human Human object is returned with an updated configuration. The dictionary, taken from H.CFG, has the following key's values updated: ``CA1elevation``, ``CA1abduction``, ``A1A2flexion``, ``CB1elevation``, ``CB1abduction``, ``B1B2flexion``, ``PJ1elevation``, ``PJ1abduction``, ``J1J2flexion``, ``PK1elevation``, ``PK1abduction``, ``K1K2flexion``. Notes ----- Requires that the bike object has a raw data text input file that contains the measurements necessary to situate a rider on the bike (i.e. ``<pathToData>/bicycles/<short name>/RawData/<short name>Measurements.txt``). .. _yeadon documentation : http://packages.python.org/yeadon """ # create human using input measurements and configuration files H = yeadon.human(yeadonMeas, yeadonCFG) measuredPar = remove_uncertainties(measuredPar) benchmarkPar = remove_uncertainties(benchmarkPar) # for simplicity of code, unpack some variables # yeadon configuration (joint angles) CFG = H.CFG # bottom bracket height hbb = measuredPar['hbb'] # chain stay length Lcs = measuredPar['lcs'] # rear wheel radius rR = benchmarkPar['rR'] # front wheel radius rF = benchmarkPar['rF'] # seat post length Lsp = measuredPar['lsp'] # seat tube length Lst = measuredPar['lst'] # seat tube angle lamst = measuredPar['lamst'] # handlebar width whb = measuredPar['whb'] # distance from rear wheel hub to hand LhbR = measuredPar['LhbR'] # distance from front wheel hub to hand LhbF = measuredPar['LhbF'] # wheelbase w = benchmarkPar['w'] # intermediate quantities # distance between wheel centers D = np.sqrt(w**2 + (rR - rF)**2) # length of the projection of the hub to handlebar vectors into the plane # of the bike dhbR = np.sqrt(LhbR**2 - (whb / 2)**2) dhbF = np.sqrt(LhbF**2 - (whb / 2)**2) # angle with vertex at rear hub, from horizontal "down" to front hub alpha = np.arcsin( (rR - rF) / D ) # angle at rear hub of the dhbR-dhbF-D triangle (side-side-side) gamma = np.arccos( (dhbR**2 + D**2 - dhbF**2) / (2 * dhbR * D) ) # position of bottom bracket center with respect to rear wheel contact # point pos_bb = np.array([[np.sqrt(Lcs**2 - (rR - hbb)**2)], [0.], [-hbb]]) # vector from bottom bracket to seat vec_seat = -(Lst + Lsp) * np.array([[np.cos(lamst)], [0.], [np.sin(lamst)]]) # position of seat with respect to rear wheel contact point pos_seat = pos_bb + vec_seat # vector (out of plane) from plane to right hand on the handlebars vec_hb_out = np.array([[0.], [whb / 2.], [0.]]) # vector (in plane) from rear wheel contact point to in-plane # location of hands vec_hb_in = np.array([[dhbR * np.cos(gamma - alpha)], [0.], [-rR - dhbR * np.sin(gamma - alpha)]]) # desired position of right hand with respect to rear wheel contact point pos_handr = vec_hb_out + vec_hb_in # desired position of left hand with respect to rear wheel contact point pos_handl = -vec_hb_out + vec_hb_in # time to calculate the relevant quantities! # vector from seat to feet, ignoring out-of-plane distance vec_legs = -vec_seat # move the yeadon origin to the bike seat # translation is done in bike's coordinate system H.translate_coord_sys(pos_seat) H.rotate_coord_sys((np.pi, 0., -np.pi /2.)) # left foot pos_footl = pos_bb.copy() # set the y value at the same width as the hip pos_footl[1, 0] = H.J1.pos[1, 0] # right foot pos_footr = pos_bb.copy() # set the y value at the same width as the hip pos_footr[1, 0] = H.K1.pos[1, 0] # find the distance from the hip joint to the desired position of the foot DJ = np.linalg.norm(pos_footl - H.J1.pos) DK = np.linalg.norm(pos_footr - H.K1.pos) # find the distance from the should joint to the desired position of the # hands DA = np.linalg.norm(pos_handl - H.A1.pos) DB = np.linalg.norm(pos_handr - H.B1.pos) # distance from knees to arch level dj2 = np.linalg.norm(H.j[7].pos - H.J2.pos) dk2 = np.linalg.norm(H.k[7].pos - H.K2.pos) # distance from elbow to knuckle level da2 = np.linalg.norm(H.a[6].pos - H.A2.pos) db2 = np.linalg.norm(H.b[6].pos - H.B2.pos) # error-checking to make sure limbs are long enough for rider to sit # on the bike if (H.J1.length + dj2) < DJ: print "For the given measurements, the left leg is not " \ "long enough. Left leg length is", H.J1.length + dj2, \ "m, but distance from left hip joint to bottom bracket is", \ DJ, "m." raise Exception() if (H.K1.length + dk2) < DK: print "For the given measurements, the right leg is not " \ "long enough. Right leg length is", H.K1.length + dk2, \ "m, but distance from right hip joint to bottom bracket is", \ DK, "m." raise Exception() if (H.A1.length + da2) < DA: print "For the given configuration, the left arm is not " \ "long enough. Left arm length is", H.A1.length + da2, \ "m, but distance from shoulder to left hand is", DA ,"m." raise Exception() if (H.B1.length + db2) < DB: print "For the given configuration, the right arm is not " \ "long enough. Right arm length is", H.B1.length + db2, \ "m, but distance from shoulder to right hand is", DB ,"m." raise Exception() # joint angle time # legs first. torso cannot have twist # left leg tempangle, CFG['J1J2flexion'] =\ calc_two_link_angles(H.J1.length, dj2, DJ) tempangle2 = vec_angle(np.array([[0,0,1]]).T, vec_legs) CFG['PJ1flexion'] = tempangle + tempangle2 + CFG['somersalt'] # right leg tempangle,CFG['K1K2flexion'] =\ calc_two_link_angles(H.K1.length, dk2, DK) CFG['PK1flexion'] = tempangle + tempangle2 + CFG['somersalt'] # arms second. only somersalt can be specified, other torso # configuration variables must be zero def dist_hand_handle(angles, r_sh_hb, r_sh_h): """ Returns the norm of the difference of the vector from the shoulder to the hand to the vector from the shoulder to the handlebar (i.e. the distance from the hand to the handlebar). Parameters ---------- angles : array_like, shape(2,) The first angle is the elevation angle of the arm and the second is the abduction angle, both relative to the chest using euler 1-2-3 angles. r_sh_hb : numpy.matrix, shape(3,1) The vector from the shoulder to the handlebar expressed in the chest reference frame. r_sh_h : numpy.matrix, shape(3,1) The vector from the shoulder to the hand (elbow is bent) expressed in the arm reference frame. Returns ------- distance : float The distance from the handlebar point to the hand. """ # these are euler rotation functions def x_rot(angle): sa = np.sin(angle) ca = np.cos(angle) Rx = np.matrix([[1., 0. , 0.], [0., ca, sa], [0., -sa, ca]]) return Rx def y_rot(angle): sa = np.sin(angle) ca = np.cos(angle) Ry = np.matrix([[ca, 0. , -sa], [0., 1., 0.], [sa, 0., ca]]) return Ry elevation = angles[0] abduction = angles[1] # create the rotation matrix of A (arm) in C (chest) R_A_C = y_rot(abduction) * x_rot(elevation) # express the vector from the shoulder to the hand in the C (chest) # refernce frame r_sh_h = R_A_C.T * r_sh_h return np.linalg.norm(r_sh_h - r_sh_hb) # left arm ########## tempangle, CFG['A1A2flexion'] =\ calc_two_link_angles(H.A1.length, da2, DA) # this is the angle between the vector from the seat to the shoulder center # and the vector from the shoulder center to the handlebar center tempangle2 = vec_angle(vec_project(H.A1.pos - pos_seat, 1), vec_project(pos_handl - H.A1.pos, 1)) # the somersault angle plus the angle between the z unit vector and the # vector from the left shoulder to the left hand tempangle2 = CFG['somersalt'] + vec_angle(np.array([[0, 0, 1]]).T, pos_handl - H.A1.pos) # subtract the angle due to the arm not being straight CFG['CA1elevation'] = tempangle2 - tempangle # the angle between the vector from the shoulder to the handlebar and its # projection in the sagittal plane CFG['CA1abduction'] = vec_angle(pos_handl - H.A1.pos, vec_project(pos_handl - H.A1.pos, 1)) # vector from the left shoulder to the left handlebar expressed in the # benchmark coordinates r_sh_hb = pos_handl - H.A1.pos # express r_sh_hb in the chest frame R_C_N = H.C.RotMat.T # transpose because Chris defined opposite my convention r_sh_hb = R_C_N * r_sh_hb # vector from the left shoulder to the hand (elbow bent) expressed in the # chest frame r_sh_h = np.mat([[0.], [-da2 * np.sin(CFG['A1A2flexion'])], [(-(H.A1.length + da2 * np.cos(CFG['A1A2flexion'])))]]) # chris defines his rotations relative to the arm coordinates but the # dist_hand_handle function is relative to the chest coordinates, thus the # negative guess guess = np.array([-CFG['CA1elevation'], -CFG['CA1abduction']]) # home in on the exact solution elevation, abduction = fmin(dist_hand_handle, guess, args=(r_sh_hb, r_sh_h), disp=False) # set the angles CFG['CA1elevation'], CFG['CA1abduction'] = -elevation, -abduction # right arm ########### tempangle, CFG['B1B2flexion'] =\ calc_two_link_angles(H.B1.length, db2, DB) tempangle2 = vec_angle(vec_project(H.B1.pos - pos_seat, 1), vec_project(pos_handr - H.B1.pos, 1)) tempangle2 = CFG['somersalt'] + vec_angle(np.array([[0,0,1]]).T, pos_handr - H.B1.pos) CFG['CB1elevation'] = tempangle2 - tempangle CFG['CB1abduction'] = vec_angle(pos_handr - H.B1.pos, vec_project(pos_handr - H.B1.pos, 1)) # vector from the left shoulder to the left handlebar expressed in the # benchmark coordinates r_sh_hb = pos_handr - H.B1.pos # express r_sh_hb in the chest frame R_C_N = H.C.RotMat.T # transpose because Chris defined opposite my convention r_sh_hb = R_C_N * r_sh_hb # vector from the left shoulder to the hand (elbow bent) expressed in the # chest frame r_sh_h = np.mat([[0.], [-db2 * np.sin(CFG['B1B2flexion'])], [(-(H.B1.length + db2 * np.cos(CFG['B1B2flexion'])))]]) # chris defines his rotations relative to the arm coordinates but the # dist_hand_handle function is relative to the chest coordinates, thus the # one negative guess guess = np.array([-CFG['CB1elevation'], CFG['CB1abduction']]) # home in on the exact solution elevation, abduction = fmin(dist_hand_handle, guess, args=(r_sh_hb, r_sh_h), disp=False) # set the angles CFG['CB1elevation'], CFG['CB1abduction'] = -elevation, abduction # assign configuration to human and check that the solution worked H.set_CFG_dict(CFG) # make sure that the arms and legs actually went where they were supposed # to within 3 decimal places def limb_warning(limbName, desiredPos, actualPos, desiredLen, actualLen, dc): # round the values desiredPos = np.round(desiredPos, dc) actualPos = np.round(actualPos, dc) desiredLen = np.round(desiredLen, dc) actualLen = np.round(actualLen, dc) message = '-' * 79 + '\n' message += (limbName + "'s actual position does not match its " + "desired position to {} decimal places.\n").format(str(decimal)) message += limbName + " desired position:\n{}\n".format(desiredPos) message += limbName + " actual position:\n{}\n".format(actualPos) message += limbName + " desired base to end distance: {}\n".format(desiredLen) message += limbName + " actual base to end distance: {}\n".format(actualLen) message += '-' * 79 if (actualPos != desiredPos).any(): print message decimal = 3 limb_warning('Left leg', pos_footl, H.j[7].pos, DJ, np.linalg.norm(H.j[7].pos - H.J1.pos), decimal) limb_warning('Right leg', pos_footr, H.k[7].pos, DK, np.linalg.norm(H.k[7].pos - H.K1.pos), decimal) limb_warning('Left arm', pos_handl, H.a[6].pos, DA, np.linalg.norm(H.a[6].pos - H.A1.pos), decimal) limb_warning('Left arm', pos_handr, H.b[6].pos, DB, np.linalg.norm(H.b[6].pos - H.B1.pos), decimal) # draw rider for fun, but possibly to check results aren't crazy if drawrider==True: H.draw_visual(forward=(0,-1,0),up=(0,0,-1)) H.draw_vector('origin',pos_footl) H.draw_vector('origin',pos_footr) H.draw_vector('origin',pos_handl) H.draw_vector('origin',pos_handr) H.draw_vector('origin',H.A2.endpos) H.draw_vector('origin',H.A2.endpos,(0,0,1)) H.draw_vector('origin',H.B2.endpos,(0,0,1)) return H