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 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 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
