def inertia_torque(rb, frame, kde_map=None, constraint_map=None, uaux=None):
# get central inertia
# I_S/O = I_S/S* + I_S*/O
I = rb.inertia[0] - inertia_of_point_mass(
rb.mass, rb.masscenter.pos_from(rb.inertia[1]), rb.frame)
alpha = rb.frame.ang_acc_in(frame)
omega = rb.frame.ang_vel_in(frame)
if not isinstance(alpha, Vector) and alpha == 0:
alpha = Vector([])
if not isinstance(omega, Vector) and omega == 0:
omega = Vector([])
if uaux is not None:
# auxilliary speeds do not change alpha, omega
# use doit() to evaluate terms such as
# Derivative(0, t) to 0.
uaux_zero = dict(zip(uaux, [0] * len(uaux)))
alpha = subs(alpha, uaux_zero)
omega = subs(omega, uaux_zero)
if kde_map is not None:
alpha = subs(alpha, kde_map)
omega = subs(omega, kde_map)
if constraint_map is not None:
alpha = subs(alpha, constraint_map)
omega = subs(omega, constraint_map)
return -dot(alpha, I) - dot(cross(omega, I), omega)
def Matrix_coord(list_vectores):
M = []
for vector in list_vectores:
coordx_v, coordy_v, coordz_v = dot(N.x, vector), dot(N.y, vector), dot(
N.z, vector)
M.append([coordx_v, coordy_v, coordz_v])
return Matrix(M)
def inertia_torque(rb, frame, kde_map=None, constraint_map=None, uaux=None):
# get central inertia
# I_S/O = I_S/S* + I_S*/O
I = rb.inertia[0] - inertia_of_point_mass(rb.mass,
rb.masscenter.pos_from(rb.inertia[1]), rb.frame)
alpha = rb.frame.ang_acc_in(frame)
omega = rb.frame.ang_vel_in(frame)
if not isinstance(alpha, Vector) and alpha == 0:
alpha = Vector([])
if not isinstance(omega, Vector) and omega == 0:
omega = Vector([])
if uaux is not None:
# auxilliary speeds do not change alpha, omega
# use doit() to evaluate terms such as
# Derivative(0, t) to 0.
uaux_zero = dict(zip(uaux, [0] * len(uaux)))
alpha = subs(alpha, uaux_zero)
omega = subs(omega, uaux_zero)
if kde_map is not None:
alpha = subs(alpha, kde_map)
omega = subs(omega, kde_map)
if constraint_map is not None:
alpha = subs(alpha, constraint_map)
omega = subs(omega, constraint_map)
return -dot(alpha, I) - dot(cross(omega, I), omega)
def main():
bus.orient(sun, "Axis", [30 / 180 * sp.pi, sun.z])
for i in range(0, n):
# for each panel, create a new reference frame.
panel_ors[i] = Panel(dimensions=[1, 1],
connections=[0, "bus", 0, 0],
frame=bus.orientnew("panel" + str(i), "Axis",
[10 / 180 * sp.pi, bus.z]))
upd = 3.6**-1 # hertz
t = 1 / upd
omega_0 = 0
for panel in panel_ors:
panel = panel_ors[panel]
c = 0
theta = sp.acos(mech.dot(panel.frame.x, bus.x).evalf()) * panel.frame.z
y_axis_theta = []
y_axis_force = []
y_axis_moment_srp = []
y_axis_moment_spr = []
bus_location = 1.5 * sun.x # location in AU referenced on the sun frame.
while True: # get_angle(panel.frame, bus) > -0.18: # When the x-axis of of the bus projects on the y-axis of the bus, the panel latches into place.
print("current angle is " + str(mech.dot(theta, bus.z).evalf()) +
"\n")
f = panel.area * calc_srp(bus_location, bus, state=0)
angled_force = calc_torque(panel, f)
spring = get_spring_moment(panel.frame, bus)
alpha = (panel.torque + spring) / panel.MOI
d_theta = omega_0 * t + alpha * t * t / 2
theta += d_theta
c += 1
omega_0 = omega_0 + alpha * t
panel.frame.orient(bus, "Axis",
[mech.dot(theta, panel.frame.z), bus.z])
print(str(theta) + ", " + str(omega_0) + ", " + str(c))
y_axis_theta.append(theta.magnitude().evalf())
y_axis_force.append(angled_force)
y_axis_moment_spr.append(spring.magnitude().evalf())
y_axis_moment_srp.append(panel.torque.magnitude().evalf())
print("Solution found after " + str(c) + " iterations")
#x_axis = list(frange(0, t*c, t))
x_axis = np.linspace(0.0, c * t, c)
fig, ax = plt.subplots() # Create a figure and an axes.
ax.plot(x_axis, y_axis_theta,
label='theta') # Plot some data on the axes.
ax.plot(x_axis, y_axis_force,
label='force') # Plot more data on the axes...
ax.plot(x_axis, y_axis_moment_spr, label='moment_spr')
ax.plot(x_axis, y_axis_moment_srp, label='moment_srp')
#ax.plot(x_axis, x**3, label='cubic') # ... and some more.
ax.set_xlabel('x label') # Add an x-label to the axes.
ax.set_ylabel('y label') # Add a y-label to the axes.
ax.set_title("Simple Plot") # Add a title to the axes.
ax.legend() # Add a legend.
plt.show()
def generalized_inertia_forces(partials,
bodies,
kde_map=None,
constraint_map=None,
uaux=None):
# use the same frame used in calculating partial velocities
ulist = partials.ulist
frame = partials.frame
if uaux is not None:
uaux_zero = dict(zip(uaux, [0] * len(uaux)))
Fr_star = [0] * len(ulist)
for b in bodies:
if isinstance(b, RigidBody):
p = b.masscenter
m = b.mass
elif isinstance(b, Particle):
p = b.point
m = b.mass
else:
raise TypeError('{0} is not a RigidBody or Particle'.format(b))
# get acceleration of point
a = p.acc(frame)
if uaux is not None:
# auxilliary speeds do not change a
a = subs(a, uaux_zero)
if kde_map is not None:
a = subs(a, kde_map)
if constraint_map is not None:
a = subs(a, constraint_map)
# get T* for RigidBodys
if isinstance(b, RigidBody):
T_star = _calculate_T_star(b, frame, kde_map, constraint_map, uaux)
for i, u in enumerate(ulist):
force_term = 0
torque_term = 0
# inertia force term
force_term = dot(partials[p][u], -m * a)
# add inertia torque term for RigidBodys
if isinstance(b, RigidBody):
torque_term = dot(partials[b.frame][u], T_star)
# auxilliary speeds have no effect on original inertia forces
if uaux is not None and u not in uaux:
force_term = subs(force_term, uaux_zero)
torque_term = subs(torque_term, uaux_zero)
Fr_star[i] += force_term + torque_term
return Fr_star, ulist
def get_angle(frame1, frame2):
angle = sp.asin(mech.dot(frame1.y.express(frame2), frame2.x).evalf())
sector = mech.dot(frame1.x, frame2.x).evalf()
sign = sector / abs(sector)
if sign == -1.0:
angle = sp.pi - angle
angle = -angle.evalf()
if threshold(angle, -sp.pi):
angle = sp.pi
return angle # positive when the y axis turns anti-CW from frame2's y-axis.
def generalized_inertia_forces(partials, bodies,
kde_map=None, constraint_map=None,
uaux=None):
# use the same frame used in calculating partial velocities
ulist = partials.ulist
frame = partials.frame
if uaux is not None:
uaux_zero = dict(zip(uaux, [0] * len(uaux)))
Fr_star = [0] * len(ulist)
for b in bodies:
if isinstance(b, RigidBody):
p = b.masscenter
m = b.mass
elif isinstance(b, Particle):
p = b.point
m = b.mass
else:
raise TypeError('{0} is not a RigidBody or Particle'.format(b))
# get acceleration of point
a = p.acc(frame)
if uaux is not None:
# auxilliary speeds do not change a
a = subs(a, uaux_zero)
if kde_map is not None:
a = subs(a, kde_map)
if constraint_map is not None:
a = subs(a, constraint_map)
# get T* for RigidBodys
if isinstance(b, RigidBody):
T_star = _calculate_T_star(b, frame, kde_map, constraint_map, uaux)
for i, u in enumerate(ulist):
force_term = 0
torque_term = 0
# inertia force term
force_term = dot(partials[p][u], -m*a)
# add inertia torque term for RigidBodys
if isinstance(b, RigidBody):
torque_term = dot(partials[b.frame][u], T_star)
# auxilliary speeds have no effect on original inertia forces
if uaux is not None and u not in uaux:
force_term = subs(force_term, uaux_zero)
torque_term = subs(torque_term, uaux_zero)
Fr_star[i] += force_term + torque_term
return Fr_star, ulist
def test_dot():
assert dot(A.x, A.x) == 1
assert dot(A.x, A.y) == 0
assert dot(A.x, A.z) == 0
assert dot(A.y, A.x) == 0
assert dot(A.y, A.y) == 1
assert dot(A.y, A.z) == 0
assert dot(A.z, A.x) == 0
assert dot(A.z, A.y) == 0
assert dot(A.z, A.z) == 1
def test_dot():
assert dot(A.x, A.x) == 1
assert dot(A.x, A.y) == 0
assert dot(A.x, A.z) == 0
assert dot(A.y, A.x) == 0
assert dot(A.y, A.y) == 1
assert dot(A.y, A.z) == 0
assert dot(A.z, A.x) == 0
assert dot(A.z, A.y) == 0
assert dot(A.z, A.z) == 1
def angle_between_vectors(a, b):
"""Return the minimum angle between two vectors. The angle returned for
vectors a and -a is 0.
"""
angle = (acos(dot(a, b) / (a.magnitude() * b.magnitude())) * 180 /
pi).evalf()
return min(angle, 180 - angle)
def angle_between_vectors(a, b):
"""Return the minimum angle between two vectors. The angle returned for
vectors a and -a is 0.
"""
angle = (acos(dot(a, b)/(a.magnitude() * b.magnitude())) *
180 / pi).evalf()
return min(angle, 180 - angle)
def calc_srp(bus_location, bus):
alpha = sp.acos(mech.dot(bus_location.normalize(), bus.x)).evalf()
pressure_ref = 4.56E-6 # n/m^2 at 1 AU
Rs = bus_location.magnitude()
pressure = pressure_ref * (1 / Rs)**2 * sp.cos(alpha)
pressure = pressure * bus.y
return pressure
def test_aux():
# Same as above, except we have 2 auxiliary speeds for the ground contact
# point, which is known to be zero. In one case, we go through then
# substitute the aux. speeds in at the end (they are zero, as well as their
# derivative), in the other case, we use the built-in auxiliary speed part
# of KanesMethod. The equations from each should be the same.
q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2')
u4d, u5d = dynamicsymbols('u4, u5', 1)
r, m, g = symbols('r m g')
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Axis', [q1, N.z])
L = Y.orientnew('L', 'Axis', [q2, Y.x])
R = L.orientnew('R', 'Axis', [q3, L.y])
w_R_N_qd = R.ang_vel_in(N)
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
C = Point('C')
C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x))
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
Dmc.a2pt_theory(C, N, R)
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]
ForceList = [(Dmc, -m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
BodyList = [BodyD]
KM = KanesMethod(N,
q_ind=[q1, q2, q3],
u_ind=[u1, u2, u3, u4, u5],
kd_eqs=kd)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar) = KM.kanes_equations(ForceList, BodyList)
fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
KM2 = KanesMethod(N,
q_ind=[q1, q2, q3],
u_ind=[u1, u2, u3],
kd_eqs=kd,
u_auxiliary=[u4, u5])
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr2, frstar2) = KM2.kanes_equations(ForceList, BodyList)
fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar.simplify()
frstar2.simplify()
assert (fr - fr2).expand() == Matrix([0, 0, 0, 0, 0])
assert (frstar - frstar2).expand() == Matrix([0, 0, 0, 0, 0])
def main():
for i in range(0, n):
# for each panel, create a new reference frame.
panel_ors[i] = Panel(
dimensions=[1,1],
connections=[0,"bus",0, 0],
frame = bus.orientnew(str(i), "Axis", [80/180 * sp.pi, bus.z])
)
upd = 100 # hertz
t= 1/upd
omega_0 = 0
for panel in panel_ors:
panel = panel_ors[panel]
iter = 0
theta = sp.acos(mech.dot(panel.frame.x, bus.x).evalf()) * panel.frame.z
y_axis_theta = []
y_axis_force = []
while mech.dot(theta, bus.z).evalf() > 0: # When the x-axis of of the bus projects on the y-axis of the bus, the panel latches into place.
print("current angle is "+str(mech.dot(theta, bus.z).evalf())+"\n")
angled_force = calc_torque(panel, f)
alpha = panel.torque/ panel.MOI
d_theta = omega_0 * t + alpha*t*t/2
theta += d_theta
iter += 1
omega_0 = omega_0 + alpha*t
panel.frame.orient(bus, "Axis", [mech.dot(theta, panel.frame.z), bus.z])
print(str(theta)+", "+str(omega_0)+", "+str(iter))
y_axis_theta.append(theta.magnitude().evalf())
y_axis_force.append(sp.Abs(angled_force))
print("Solution found after "+str(iter)+" iterations")
#x_axis = list(frange(0, t*iter, t))
x_axis = np.linspace(0.0, iter*t, iter)
fig, ax = plt.subplots() # Create a figure and an axes.
ax.plot(x_axis, y_axis_theta, label='theta') # Plot some data on the axes.
ax.plot(x_axis, y_axis_force, label='force') # Plot more data on the axes...
#ax.plot(x_axis, x**3, label='cubic') # ... and some more.
ax.set_xlabel('x label') # Add an x-label to the axes.
ax.set_ylabel('y label') # Add a y-label to the axes.
ax.set_title("Simple Plot") # Add a title to the axes.
ax.legend() # Add a legend.
plt.show()
def calc_torque(panel, force, bus):
force_z = mech.dot(force, bus.y) * sp.cos(get_angle(panel.frame, bus))
if force_z > 0:
force_z = force_z * (
1 + panel.state
) # if the light hits on the front of the panel, use state to determine a coefficient of reflection.
#force_z = mech.dot(force, panel.frame.y).evalf()
torque_zz = -panel.dimensions[0] * 3**(1 / 2) / 6 * force_z
panel.torque = 0 * panel.frame.y + 0 * panel.frame.x + torque_zz * panel.frame.z
return force_z
def test_aux():
# Same as above, except we have 2 auxiliary speeds for the ground contact
# point, which is known to be zero. In one case, we go through then
# substitute the aux. speeds in at the end (they are zero, as well as their
# derivative), in the other case, we use the built-in auxiliary speed part
# of KanesMethod. The equations from each should be the same.
q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2')
u4d, u5d = dynamicsymbols('u4, u5', 1)
r, m, g = symbols('r m g')
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Axis', [q1, N.z])
L = Y.orientnew('L', 'Axis', [q2, Y.x])
R = L.orientnew('R', 'Axis', [q3, L.y])
w_R_N_qd = R.ang_vel_in(N)
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
C = Point('C')
C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x))
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
Dmc.a2pt_theory(C, N, R)
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]
ForceList = [(Dmc, - m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
BodyList = [BodyD]
KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3, u4, u5],
kd_eqs=kd)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar) = KM.kanes_equations(ForceList, BodyList)
fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
KM2 = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd,
u_auxiliary=[u4, u5])
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr2, frstar2) = KM2.kanes_equations(ForceList, BodyList)
fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar.simplify()
frstar2.simplify()
assert (fr - fr2).expand() == Matrix([0, 0, 0, 0, 0])
assert (frstar - frstar2).expand() == Matrix([0, 0, 0, 0, 0])
def inertia_coefficient_contribution(body, partials, r, s):
"""Returns the contribution of a rigid body (or particle) to the inertia
coefficient m_rs of a system.
'body' is an instance of a RigidBody or Particle.
'partials' is an instance of a PartialVelocity.
'r' is the first generalized speed.
's' is the second generlized speed.
"""
if isinstance(body, Particle):
m_rs = body.mass * dot(partials[body.point][r],
partials[body.point][s])
elif isinstance(body, RigidBody):
m_rs = body.mass * dot(partials[body.masscenter][r],
partials[body.masscenter][s])
m_rs += dot(dot(partials[body.frame][r], body.central_inertia),
partials[body.frame][s])
else:
raise TypeError(('{0} is not a RigidBody or Particle.').format(body))
return m_rs
def inertia_coefficient_contribution(body, partials, r, s):
"""Returns the contribution of a rigid body (or particle) to the inertia
coefficient m_rs of a system.
'body' is an instance of a RigidBody or Particle.
'partials' is an instance of a PartialVelocity.
'r' is the first generalized speed.
's' is the second generlized speed.
"""
if isinstance(body, Particle):
m_rs = body.mass * dot(partials[body.point][r],
partials[body.point][s])
elif isinstance(body, RigidBody):
m_rs = body.mass * dot(partials[body.masscenter][r],
partials[body.masscenter][s])
m_rs += dot(dot(partials[body.frame][r], body.central_inertia),
partials[body.frame][s])
else:
raise TypeError(('{0} is not a RigidBody or Particle.').format(body))
return m_rs
def get_angle(frame1, frame2):
angle = sp.acos(mech.dot(frame1.y, frame2.y)).evalf()
try:
projection = frame1.y.express(frame2).args[0][0][0] // abs(
frame1.y.express(frame2).args[0][0][0]
) # If the y axis of the panel has a non-zero projection on the x-axis of the bus, it is at an angle. It falls -ve if CCW, it falls +ve if CW.
except ZeroDivisionError:
projection = 1 # if the angle of the panel is perfectly 0, assume CW direction.
if projection > 0:
angle = -angle
return angle
def Volumen_CP(a1, a2, a3):
"""
Parameters
----------
a1 : 1 vector de la base VP
a2 : 2 vector de la base VP
a3 : 3 vector de la base VP
Returns
-------
volumen de 1 CP
"""
return dot(a1, cross(a2, a3))
def generalized_active_forces(partials, forces, uaux=None):
# use the same frame used in calculating partial velocities
ulist = partials.ulist
if uaux is not None:
uaux_zero = dict(zip(uaux, [0] * len(uaux)))
Fr = [0] * len(ulist)
for pf in forces:
p = pf[0]
f = pf[1]
for i, u in enumerate(ulist):
if partials[p][u] != 0 and f != 0:
r = dot(partials[p][u], f)
# if more than 2 args, 3rd is an integral function, where the
# input is the integrand
if len(pf) > 2:
r = pf[2](r)
# auxilliary speeds have no effect on original active forces
if uaux is not None and u not in uaux:
r = subs(r, uaux_zero)
Fr[i] += r
return Fr, ulist
def generalized_active_forces(partials, forces, uaux=None):
# use the same frame used in calculating partial velocities
ulist = partials.ulist
if uaux is not None:
uaux_zero = dict(zip(uaux, [0] * len(uaux)))
Fr = [0] * len(ulist)
for pf in forces:
p = pf[0] # first arg is point/rf
f = pf[1] # second arg is force/torque
for i, u in enumerate(ulist):
if partials[p][u] != 0 and f != 0:
r = dot(partials[p][u], f)
# if more than 2 args, 3rd is an integral function, where the
# input is the integrand
if len(pf) > 2:
r = pf[2](r)
# auxilliary speeds have no effect on original active forces
if uaux is not None and u not in uaux:
r = subs(r, uaux_zero)
Fr[i] += r
return Fr, ulist
R = L.orientnew('R', 'Axis', [q[2], L.y]) # Rattleback body fixed frame
I = inertia(R, Ixx, Iyy, Izz, Ixy, Iyz, Ixz) # Inertia dyadic
# Angular velocity using u's as body fixed measure numbers of angular velocity
R.set_ang_vel(N, u[0]*R.x + u[1]*R.y + u[2]*R.z)
# Rattleback ground contact point
P = Point('P')
P.set_vel(N, ua[0]*Y.x + ua[1]*Y.y + ua[2]*Y.z)
# Rattleback paraboloid -- parameterize coordinates of contact point by the
# roll and pitch angles, and the geometry
# TODO: FIXME!!!
# f(x,y,z) = a*x**2 + b*y**2 + z - c
mu = [dot(rk, Y.z) for rk in R]
rx = mu[0]/mu[2]/2/a
ry = mu[1]/mu[2]/2/b
rz = 1 - (mu[0]**2/a + mu[1]**2/b)*(1/2/mu[2])**2
# Locate origin of parabaloid coordinate system relative to contact point
O = P.locatenew('O', -rx*R.x - ry*R.y - rz*R.z)
O.set_vel(N, P.vel(N) + cross(R.ang_vel_in(N), O.pos_from(P)))
# Mass center position and velocity
RO = O.locatenew('RO', d*R.x + e*R.y)
RO.set_vel(N, P.vel(N) + cross(R.ang_vel_in(N), RO.pos_from(P)))
qd_rhs = [(u[2]*cos(q[2]) - u[0]*sin(q[2]))/cos(q[1]),
u[0]*cos(q[2]) + u[2]*sin(q[2]),
u[1] + tan(q[1])*(u[0]*sin(q[2]) - u[2]*cos(q[2])),
pC_star = pD.locatenew('C*', +b * A.z)
for p in [pA_star, pB_star, pC_star]:
p.set_vel(A, 0)
p.v2pt_theory(pD, F, A)
# points of B, C touching the plane P
pB_hat = pB_star.locatenew('B^', -R * A.x)
pC_hat = pC_star.locatenew('C^', -R * A.x)
pB_hat.set_vel(B, 0)
pC_hat.set_vel(C, 0)
pB_hat.v2pt_theory(pB_star, F, B)
pC_hat.v2pt_theory(pC_star, F, C)
# kinematic differential equations and velocity constraints
kde = [
u1 - dot(A.ang_vel_in(F), A.x), u2 - dot(pD.vel(F), A.y), u3 - q3d,
u4 - q4d, u5 - q5d
]
kde_map = solve(kde, [q1d, q2d, q3d, q4d, q5d])
vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]] + [dot(pD.vel(F), A.z)]
vc_map = solve(subs(vc, kde_map), [u3, u4, u5])
forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x)] # no friction at point Q
torques = [(A, -TB * A.z), (A, -TC * A.z), (B, TB * A.z), (C, TC * A.z)]
partials = partial_velocities(zip(*forces + torques)[0], [u1, u2],
F,
kde_map,
vc_map,
express_frame=A)
Fr, _ = generalized_active_forces(partials, forces + torques)
def inertia_matrix(dyadic, rf):
"""Return the inertia matrix of a given dyadic for a specified
reference frame.
"""
return Matrix([[dot(dot(dyadic, i), j) for j in rf] for i in rf])
def test_linearize_rolling_disc_kane():
# Symbols for time and constant parameters
t, r, m, g, v = symbols('t r m g v')
# Configuration variables and their time derivatives
q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7')
q1d, q2d, q3d, q4d, q5d, q6d = qd = [qi.diff(t) for qi in q]
# Generalized speeds and their time derivatives
u = dynamicsymbols('u:6')
u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7')
u1d, u2d, u3d, u4d, u5d, u6d = [ui.diff(t) for ui in u]
# Reference frames
N = ReferenceFrame('N') # Inertial frame
NO = Point('NO') # Inertial origin
A = N.orientnew('A', 'Axis', [q1, N.z]) # Yaw intermediate frame
B = A.orientnew('B', 'Axis', [q2, A.x]) # Lean intermediate frame
C = B.orientnew('C', 'Axis', [q3, B.y]) # Disc fixed frame
CO = NO.locatenew('CO', q4*N.x + q5*N.y + q6*N.z) # Disc center
# Disc angular velocity in N expressed using time derivatives of coordinates
w_c_n_qd = C.ang_vel_in(N)
w_b_n_qd = B.ang_vel_in(N)
# Inertial angular velocity and angular acceleration of disc fixed frame
C.set_ang_vel(N, u1*B.x + u2*B.y + u3*B.z)
# Disc center velocity in N expressed using time derivatives of coordinates
v_co_n_qd = CO.pos_from(NO).dt(N)
# Disc center velocity in N expressed using generalized speeds
CO.set_vel(N, u4*C.x + u5*C.y + u6*C.z)
# Disc Ground Contact Point
P = CO.locatenew('P', r*B.z)
P.v2pt_theory(CO, N, C)
# Configuration constraint
f_c = Matrix([q6 - dot(CO.pos_from(P), N.z)])
# Velocity level constraints
f_v = Matrix([dot(P.vel(N), uv) for uv in C])
# Kinematic differential equations
kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
[dot(v_co_n_qd - CO.vel(N), uv) for uv in N])
qdots = solve(kindiffs, qd)
# Set angular velocity of remaining frames
B.set_ang_vel(N, w_b_n_qd.subs(qdots))
C.set_ang_acc(N, C.ang_vel_in(N).dt(B) + cross(B.ang_vel_in(N), C.ang_vel_in(N)))
# Active forces
F_CO = m*g*A.z
# Create inertia dyadic of disc C about point CO
I = (m * r**2) / 4
J = (m * r**2) / 2
I_C_CO = inertia(C, I, J, I)
Disc = RigidBody('Disc', CO, C, m, (I_C_CO, CO))
BL = [Disc]
FL = [(CO, F_CO)]
KM = KanesMethod(N, [q1, q2, q3, q4, q5], [u1, u2, u3], kd_eqs=kindiffs,
q_dependent=[q6], configuration_constraints=f_c,
u_dependent=[u4, u5, u6], velocity_constraints=f_v)
with warns_deprecated_sympy():
(fr, fr_star) = KM.kanes_equations(FL, BL)
# Test generalized form equations
linearizer = KM.to_linearizer()
assert linearizer.f_c == f_c
assert linearizer.f_v == f_v
assert linearizer.f_a == f_v.diff(t)
sol = solve(linearizer.f_0 + linearizer.f_1, qd)
for qi in qd:
assert sol[qi] == qdots[qi]
assert simplify(linearizer.f_2 + linearizer.f_3 - fr - fr_star) == Matrix([0, 0, 0])
# Perform the linearization
# Precomputed operating point
q_op = {q6: -r*cos(q2)}
u_op = {u1: 0,
u2: sin(q2)*q1d + q3d,
u3: cos(q2)*q1d,
u4: -r*(sin(q2)*q1d + q3d)*cos(q3),
u5: 0,
u6: -r*(sin(q2)*q1d + q3d)*sin(q3)}
qd_op = {q2d: 0,
q4d: -r*(sin(q2)*q1d + q3d)*cos(q1),
q5d: -r*(sin(q2)*q1d + q3d)*sin(q1),
q6d: 0}
ud_op = {u1d: 4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5,
u2d: 0,
u3d: 0,
u4d: r*(sin(q2)*sin(q3)*q1d*q3d + sin(q3)*q3d**2),
u5d: r*(4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5),
u6d: -r*(sin(q2)*cos(q3)*q1d*q3d + cos(q3)*q3d**2)}
A, B = linearizer.linearize(op_point=[q_op, u_op, qd_op, ud_op], A_and_B=True, simplify=True)
upright_nominal = {q1d: 0, q2: 0, m: 1, r: 1, g: 1}
# Precomputed solution
A_sol = Matrix([[0, 0, 0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0],
[sin(q1)*q3d, 0, 0, 0, 0, -sin(q1), -cos(q1), 0],
[-cos(q1)*q3d, 0, 0, 0, 0, cos(q1), -sin(q1), 0],
[0, Rational(4, 5), 0, 0, 0, 0, 0, 6*q3d/5],
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, -2*q3d, 0, 0]])
B_sol = Matrix([])
# Check that linearization is correct
assert A.subs(upright_nominal) == A_sol
assert B.subs(upright_nominal) == B_sol
# Check eigenvalues at critical speed are all zero:
assert A.subs(upright_nominal).subs(q3d, 1/sqrt(3)).eigenvals() == {0: 8}
def test_Vector():
assert A.x != A.y
assert A.y != A.z
assert A.z != A.x
v1 = x*A.x + y*A.y + z*A.z
v2 = x**2*A.x + y**2*A.y + z**2*A.z
v3 = v1 + v2
v4 = v1 - v2
assert isinstance(v1, Vector)
assert dot(v1, A.x) == x
assert dot(v1, A.y) == y
assert dot(v1, A.z) == z
assert isinstance(v2, Vector)
assert dot(v2, A.x) == x**2
assert dot(v2, A.y) == y**2
assert dot(v2, A.z) == z**2
assert isinstance(v3, Vector)
# We probably shouldn't be using simplify in dot...
assert dot(v3, A.x) == x**2 + x
assert dot(v3, A.y) == y**2 + y
assert dot(v3, A.z) == z**2 + z
assert isinstance(v4, Vector)
# We probably shouldn't be using simplify in dot...
assert dot(v4, A.x) == x - x**2
assert dot(v4, A.y) == y - y**2
assert dot(v4, A.z) == z - z**2
def test_rolling_disc():
# Rolling Disc Example
# Here the rolling disc is formed from the contact point up, removing the
# need to introduce generalized speeds. Only 3 configuration and three
# speed variables are need to describe this system, along with the disc's
# mass and radius, and the local gravity (note that mass will drop out).
q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
r, m, g = symbols('r m g')
# The kinematics are formed by a series of simple rotations. Each simple
# rotation creates a new frame, and the next rotation is defined by the new
# frame's basis vectors. This example uses a 3-1-2 series of rotations, or
# Z, X, Y series of rotations. Angular velocity for this is defined using
# the second frame's basis (the lean frame).
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Axis', [q1, N.z])
L = Y.orientnew('L', 'Axis', [q2, Y.x])
R = L.orientnew('R', 'Axis', [q3, L.y])
w_R_N_qd = R.ang_vel_in(N)
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
# This is the translational kinematics. We create a point with no velocity
# in N; this is the contact point between the disc and ground. Next we form
# the position vector from the contact point to the disc's center of mass.
# Finally we form the velocity and acceleration of the disc.
C = Point('C')
C.set_vel(N, 0)
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
# This is a simple way to form the inertia dyadic.
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
# Kinematic differential equations; how the generalized coordinate time
# derivatives relate to generalized speeds.
kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]
# Creation of the force list; it is the gravitational force at the mass
# center of the disc. Then we create the disc by assigning a Point to the
# center of mass attribute, a ReferenceFrame to the frame attribute, and mass
# and inertia. Then we form the body list.
ForceList = [(Dmc, -m * g * Y.z)]
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
BodyList = [BodyD]
# Finally we form the equations of motion, using the same steps we did
# before. Specify inertial frame, supply generalized speeds, supply
# kinematic differential equation dictionary, compute Fr from the force
# list and Fr* from the body list, compute the mass matrix and forcing
# terms, then solve for the u dots (time derivatives of the generalized
# speeds).
KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd)
with warns_deprecated_sympy():
KM.kanes_equations(ForceList, BodyList)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
kdd = KM.kindiffdict()
rhs = rhs.subs(kdd)
rhs.simplify()
assert rhs.expand() == Matrix([
(6 * u2 * u3 * r - u3**2 * r * tan(q2) + 4 * g * sin(q2)) / (5 * r),
-2 * u1 * u3 / 3, u1 * (-2 * u2 + u3 * tan(q2))
]).expand()
assert simplify(KM.rhs() -
KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(
6, 1)
# This code tests our output vs. benchmark values. When r=g=m=1, the
# critical speed (where all eigenvalues of the linearized equations are 0)
# is 1 / sqrt(3) for the upright case.
A = KM.linearize(A_and_B=True)[0]
A_upright = A.subs({
r: 1,
g: 1,
m: 1
}).subs({
q1: 0,
q2: 0,
q3: 0,
u1: 0,
u3: 0
})
import sympy
assert sympy.sympify(A_upright.subs({u2: 1 / sqrt(3)})).eigenvals() == {
S.Zero: 6
}
I = inertia(R, Ixx, Iyy, Izz, Ixy, Iyz, Ixz) # Inertia dyadic
print(I.express(Y))
# Rattleback ground contact point
P = Point('P')
# Rattleback ellipsoid center location, see:
# "Realistic mathematical modeling of the rattleback", Kane, Thomas R. and
# David A. Levinson, 1982, International Journal of Non-Linear Mechanics
#mu = [dot(rk, Y.z) for rk in R]
#eps = sqrt((a*mu[0])**2 + (b*mu[1])**2 + (c*mu[2])**2)
O = P.locatenew('O', -rx*R.x - rx*R.y - rx*R.z)
RO = O.locatenew('RO', d*R.x + e*R.y + f*R.z)
w_r_n = wx*R.x + wy*R.y + wz*R.z
omega_dict = {wx: dot(qd[0]*Y.z, R.x),
wy: dot(qd[0]*Y.z, R.y),
wz: dot(qd[0]*Y.z, R.z)}
v_ro_n = cross(w_r_n, RO.pos_from(P))
a_ro_n = cross(w_r_n, v_ro_n)
mu_dict = {mu_x: dot(R.x, Y.z), mu_y: dot(R.y, Y.z), mu_z: dot(R.z, Y.z)}
#F_RO = m*g*Y.z + Fx*Y.x + Fy*Y.y + Fz*Y.z
#F_RO = Fx*R.x + Fy*R.y + Fz*R.z + m*g*Y.z
F_RO = Fx*R.x + Fy*R.y + Fz*R.z + m*g*(mu_x*R.x + mu_y*R.y + mu_z*R.z)
newton_eqn = F_RO - m*a_ro_n
force_scalars = solve([dot(newton_eqn, uv).expand() for uv in R], [Fx, Fy, Fz])
#print("v_ro_n =", v_ro_n)
#print("a_ro_n =", a_ro_n)
#print("Force scalars =", force_scalars)
pB_star = pD.locatenew('B*', -b*A.z)
pC_star = pD.locatenew('C*', +b*A.z)
for p in [pA_star, pB_star, pC_star]:
p.set_vel(A, 0)
p.v2pt_theory(pD, F, A)
# points of B, C touching the plane P
pB_hat = pB_star.locatenew('B^', -R*A.x)
pC_hat = pC_star.locatenew('C^', -R*A.x)
pB_hat.set_vel(B, 0)
pC_hat.set_vel(C, 0)
pB_hat.v2pt_theory(pB_star, F, B)
pC_hat.v2pt_theory(pC_star, F, C)
# kinematic differential equations and velocity constraints
kde = [u1 - dot(A.ang_vel_in(F), A.x),
u2 - dot(pD.vel(F), A.y),
u3 - q3d,
u4 - q4d,
u5 - q5d]
kde_map = solve(kde, [q1d, q2d, q3d, q4d, q5d])
vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]] + [dot(pD.vel(F), A.z)]
vc_map = solve(subs(vc, kde_map), [u3, u4, u5])
forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x)] # no friction at point Q
torques = [(A, -TB*A.z), (A, -TC*A.z), (B, TB*A.z), (C, TC*A.z)]
partials = partial_velocities(zip(*forces + torques)[0], [u1, u2],
F, kde_map, vc_map, express_frame=A)
Fr, _ = generalized_active_forces(partials, forces + torques)
q = [q1, q2, q3, q4, q5]
def test_Vector():
assert A.x != A.y
assert A.y != A.z
assert A.z != A.x
v1 = x * A.x + y * A.y + z * A.z
v2 = x ** 2 * A.x + y ** 2 * A.y + z ** 2 * A.z
v3 = v1 + v2
v4 = v1 - v2
assert isinstance(v1, Vector)
assert dot(v1, A.x) == x
assert dot(v1, A.y) == y
assert dot(v1, A.z) == z
assert isinstance(v2, Vector)
assert dot(v2, A.x) == x ** 2
assert dot(v2, A.y) == y ** 2
assert dot(v2, A.z) == z ** 2
assert isinstance(v3, Vector)
# We probably shouldn't be using simplify in dot...
assert dot(v3, A.x) == x ** 2 + x
assert dot(v3, A.y) == y ** 2 + y
assert dot(v3, A.z) == z ** 2 + z
assert isinstance(v4, Vector)
# We probably shouldn't be using simplify in dot...
assert dot(v4, A.x) == x - x ** 2
assert dot(v4, A.y) == y - y ** 2
assert dot(v4, A.z) == z - z ** 2
assert v1.to_matrix(A) == Matrix([[x], [y], [z]])
q = symbols("q")
B = A.orientnew("B", "Axis", (q, A.x))
assert v1.to_matrix(B) == Matrix([[x], [y * cos(q) + z * sin(q)], [-y * sin(q) + z * cos(q)]])
pP = pO.locatenew('P', q1 * A.x + q2 * A.y + q3 * A.z)
pP.set_vel(A, pP.pos_from(pO).dt(A))
# kinematic differential equations
kde_map = dict(zip(map(lambda x: x.diff(), q), u))
# forces
forces = [(pP, -beta * pP.vel(A))]
torques = [(B, -alpha * B.ang_vel_in(A))]
partials_c = partial_velocities(zip(*forces + torques)[0], u, A, kde_map)
Fr_c, _ = generalized_active_forces(partials_c, forces + torques)
dissipation_function = function_from_partials(map(
lambda x: 0 if x == 0 else -x.subs(kde_map), Fr_c),
u,
zero_constants=True)
from sympy import simplify, trigsimp
dissipation_function = trigsimp(dissipation_function)
#print('ℱ = {0}'.format(msprint(dissipation_function)))
omega2 = trigsimp(dot(B.ang_vel_in(A), B.ang_vel_in(A)).subs(kde_map))
v2 = trigsimp(dot(pP.vel(A), pP.vel(A)).subs(kde_map))
sym_map = dict(zip([omega2, v2], map(lambda x: x**2, symbols('ω v'))))
#print('ω**2 = {0}'.format(msprint(omega2)))
#print('v**2 = {0}'.format(msprint(v2)))
print('ℱ = {0}'.format(msprint(dissipation_function.subs(sym_map))))
dissipation_function_expected = (alpha * omega2 + beta * v2) / 2
assert expand(dissipation_function - dissipation_function_expected) == 0
# reference frames
A = ReferenceFrame('A')
B = A.orientnew('B', 'body', [q1, q2, q3], 'xyz')
# points B*, O
pB_star = Point('B*')
pB_star.set_vel(A, 0)
# rigidbody B
I_B_Bs = inertia(B, I11, I22, I33)
rbB = RigidBody('rbB', pB_star, B, m, (I_B_Bs, pB_star))
# kinetic energy
K = rbB.kinetic_energy(A) # velocity of point B* is zero
print('K_ω = {0}'.format(msprint(K)))
print('\nSince I11, I22, I33 are the central principal moments of inertia')
print('let I_min = I11, I_max = I33')
I_min = I11
I_max = I33
H = rbB.angular_momentum(pB_star, A)
K_min = dot(H, H) / I_max / 2
K_max = dot(H, H) / I_min / 2
print('K_ω_min = {0}'.format(msprint(K_min)))
print('K_ω_max = {0}'.format(msprint(K_max)))
print('\nI11/I33, I22/I33 =< 1, since I33 >= I11, I22, so K_ω_min <= K_ω')
print('Similarly, I22/I11, I33/I11 >= 1, '
'since I11 <= I22, I33, so K_ω_max >= K_ω')
pB_star.v2pt_theory(pP, N, B)
# P' is the same point as P but on rigidbody B instead of rigidbody A.
pP_prime = pB_star.locatenew('P\'', -(b - b_star)*B.z)
pP_prime.v2pt_theory(pB_star, N, B)
# Define point Q where B and C intersect.
pQ = pP.locatenew('Q', b*B.z)
pQ.v2pt_theory(pP, N, B)
# Define the distance between points Q, C* as c.
pC_star = pQ.locatenew('C*', c_star*C.z)
pC_star.v2pt_theory(pQ, N, C)
# configuration constraint for q2.
cc = [dot(pC_star.pos_from(pO), N.x)]
cc_map = solve(cc, q2)[1]
# kinematic differential equations
kde = [u1 - q1d, u2 - dot(B.ang_vel_in(N), N.y)]
kde_map = solve(kde, [q1d, q2d])
# velocity constraints
vc = subs([u3 - dot(pC_star.vel(N), N.z), cc[0].diff(t)], kde_map)
vc_map = solve(vc, [u2, u3])
# verify motion constraint equation match text
u2_expected = -a*cos(q1)/(b*cos(q2))*u1
u3_expected = -a/cos(q2)*(sin(q1)*cos(q2) + cos(q1)*sin(q2))*u1
assert trigsimp(vc_map[u2] - u2_expected) == 0
assert trigsimp(vc_map[u3] - u3_expected) == 0
def test_bicycle():
if ON_TRAVIS:
skip("Too slow for travis.")
# Code to get equations of motion for a bicycle modeled as in:
# J.P Meijaard, Jim M Papadopoulos, Andy Ruina and A.L Schwab. Linearized
# dynamics equations for the balance and steer of a bicycle: a benchmark
# and review. Proceedings of The Royal Society (2007) 463, 1955-1982
# doi: 10.1098/rspa.2007.1857
# Note that this code has been crudely ported from Autolev, which is the
# reason for some of the unusual naming conventions. It was purposefully as
# similar as possible in order to aide debugging.
# Declare Coordinates & Speeds
# Simple definitions for qdots - qd = u
# Speeds are: yaw frame ang. rate, roll frame ang. rate, rear wheel frame
# ang. rate (spinning motion), frame ang. rate (pitching motion), steering
# frame ang. rate, and front wheel ang. rate (spinning motion).
# Wheel positions are ignorable coordinates, so they are not introduced.
q1, q2, q4, q5 = dynamicsymbols('q1 q2 q4 q5')
q1d, q2d, q4d, q5d = dynamicsymbols('q1 q2 q4 q5', 1)
u1, u2, u3, u4, u5, u6 = dynamicsymbols('u1 u2 u3 u4 u5 u6')
u1d, u2d, u3d, u4d, u5d, u6d = dynamicsymbols('u1 u2 u3 u4 u5 u6', 1)
# Declare System's Parameters
WFrad, WRrad, htangle, forkoffset = symbols('WFrad WRrad htangle forkoffset')
forklength, framelength, forkcg1 = symbols('forklength framelength forkcg1')
forkcg3, framecg1, framecg3, Iwr11 = symbols('forkcg3 framecg1 framecg3 Iwr11')
Iwr22, Iwf11, Iwf22, Iframe11 = symbols('Iwr22 Iwf11 Iwf22 Iframe11')
Iframe22, Iframe33, Iframe31, Ifork11 = symbols('Iframe22 Iframe33 Iframe31 Ifork11')
Ifork22, Ifork33, Ifork31, g = symbols('Ifork22 Ifork33 Ifork31 g')
mframe, mfork, mwf, mwr = symbols('mframe mfork mwf mwr')
# Set up reference frames for the system
# N - inertial
# Y - yaw
# R - roll
# WR - rear wheel, rotation angle is ignorable coordinate so not oriented
# Frame - bicycle frame
# TempFrame - statically rotated frame for easier reference inertia definition
# Fork - bicycle fork
# TempFork - statically rotated frame for easier reference inertia definition
# WF - front wheel, again posses a ignorable coordinate
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Axis', [q1, N.z])
R = Y.orientnew('R', 'Axis', [q2, Y.x])
Frame = R.orientnew('Frame', 'Axis', [q4 + htangle, R.y])
WR = ReferenceFrame('WR')
TempFrame = Frame.orientnew('TempFrame', 'Axis', [-htangle, Frame.y])
Fork = Frame.orientnew('Fork', 'Axis', [q5, Frame.x])
TempFork = Fork.orientnew('TempFork', 'Axis', [-htangle, Fork.y])
WF = ReferenceFrame('WF')
# Kinematics of the Bicycle First block of code is forming the positions of
# the relevant points
# rear wheel contact -> rear wheel mass center -> frame mass center +
# frame/fork connection -> fork mass center + front wheel mass center ->
# front wheel contact point
WR_cont = Point('WR_cont')
WR_mc = WR_cont.locatenew('WR_mc', WRrad * R.z)
Steer = WR_mc.locatenew('Steer', framelength * Frame.z)
Frame_mc = WR_mc.locatenew('Frame_mc', - framecg1 * Frame.x
+ framecg3 * Frame.z)
Fork_mc = Steer.locatenew('Fork_mc', - forkcg1 * Fork.x
+ forkcg3 * Fork.z)
WF_mc = Steer.locatenew('WF_mc', forklength * Fork.x + forkoffset * Fork.z)
WF_cont = WF_mc.locatenew('WF_cont', WFrad * (dot(Fork.y, Y.z) * Fork.y -
Y.z).normalize())
# Set the angular velocity of each frame.
# Angular accelerations end up being calculated automatically by
# differentiating the angular velocities when first needed.
# u1 is yaw rate
# u2 is roll rate
# u3 is rear wheel rate
# u4 is frame pitch rate
# u5 is fork steer rate
# u6 is front wheel rate
Y.set_ang_vel(N, u1 * Y.z)
R.set_ang_vel(Y, u2 * R.x)
WR.set_ang_vel(Frame, u3 * Frame.y)
Frame.set_ang_vel(R, u4 * Frame.y)
Fork.set_ang_vel(Frame, u5 * Fork.x)
WF.set_ang_vel(Fork, u6 * Fork.y)
# Form the velocities of the previously defined points, using the 2 - point
# theorem (written out by hand here). Accelerations again are calculated
# automatically when first needed.
WR_cont.set_vel(N, 0)
WR_mc.v2pt_theory(WR_cont, N, WR)
Steer.v2pt_theory(WR_mc, N, Frame)
Frame_mc.v2pt_theory(WR_mc, N, Frame)
Fork_mc.v2pt_theory(Steer, N, Fork)
WF_mc.v2pt_theory(Steer, N, Fork)
WF_cont.v2pt_theory(WF_mc, N, WF)
# Sets the inertias of each body. Uses the inertia frame to construct the
# inertia dyadics. Wheel inertias are only defined by principle moments of
# inertia, and are in fact constant in the frame and fork reference frames;
# it is for this reason that the orientations of the wheels does not need
# to be defined. The frame and fork inertias are defined in the 'Temp'
# frames which are fixed to the appropriate body frames; this is to allow
# easier input of the reference values of the benchmark paper. Note that
# due to slightly different orientations, the products of inertia need to
# have their signs flipped; this is done later when entering the numerical
# value.
Frame_I = (inertia(TempFrame, Iframe11, Iframe22, Iframe33, 0, 0, Iframe31), Frame_mc)
Fork_I = (inertia(TempFork, Ifork11, Ifork22, Ifork33, 0, 0, Ifork31), Fork_mc)
WR_I = (inertia(Frame, Iwr11, Iwr22, Iwr11), WR_mc)
WF_I = (inertia(Fork, Iwf11, Iwf22, Iwf11), WF_mc)
# Declaration of the RigidBody containers. ::
BodyFrame = RigidBody('BodyFrame', Frame_mc, Frame, mframe, Frame_I)
BodyFork = RigidBody('BodyFork', Fork_mc, Fork, mfork, Fork_I)
BodyWR = RigidBody('BodyWR', WR_mc, WR, mwr, WR_I)
BodyWF = RigidBody('BodyWF', WF_mc, WF, mwf, WF_I)
# The kinematic differential equations; they are defined quite simply. Each
# entry in this list is equal to zero.
kd = [q1d - u1, q2d - u2, q4d - u4, q5d - u5]
# The nonholonomic constraints are the velocity of the front wheel contact
# point dotted into the X, Y, and Z directions; the yaw frame is used as it
# is "closer" to the front wheel (1 less DCM connecting them). These
# constraints force the velocity of the front wheel contact point to be 0
# in the inertial frame; the X and Y direction constraints enforce a
# "no-slip" condition, and the Z direction constraint forces the front
# wheel contact point to not move away from the ground frame, essentially
# replicating the holonomic constraint which does not allow the frame pitch
# to change in an invalid fashion.
conlist_speed = [WF_cont.vel(N) & Y.x, WF_cont.vel(N) & Y.y, WF_cont.vel(N) & Y.z]
# The holonomic constraint is that the position from the rear wheel contact
# point to the front wheel contact point when dotted into the
# normal-to-ground plane direction must be zero; effectively that the front
# and rear wheel contact points are always touching the ground plane. This
# is actually not part of the dynamic equations, but instead is necessary
# for the lineraization process.
conlist_coord = [WF_cont.pos_from(WR_cont) & Y.z]
# The force list; each body has the appropriate gravitational force applied
# at its mass center.
FL = [(Frame_mc, -mframe * g * Y.z),
(Fork_mc, -mfork * g * Y.z),
(WF_mc, -mwf * g * Y.z),
(WR_mc, -mwr * g * Y.z)]
BL = [BodyFrame, BodyFork, BodyWR, BodyWF]
# The N frame is the inertial frame, coordinates are supplied in the order
# of independent, dependent coordinates, as are the speeds. The kinematic
# differential equation are also entered here. Here the dependent speeds
# are specified, in the same order they were provided in earlier, along
# with the non-holonomic constraints. The dependent coordinate is also
# provided, with the holonomic constraint. Again, this is only provided
# for the linearization process.
KM = KanesMethod(N, q_ind=[q1, q2, q5],
q_dependent=[q4], configuration_constraints=conlist_coord,
u_ind=[u2, u3, u5],
u_dependent=[u1, u4, u6], velocity_constraints=conlist_speed,
kd_eqs=kd)
(fr, frstar) = KM.kanes_equations(FL, BL)
# This is the start of entering in the numerical values from the benchmark
# paper to validate the eigen values of the linearized equations from this
# model to the reference eigen values. Look at the aforementioned paper for
# more information. Some of these are intermediate values, used to
# transform values from the paper into the coordinate systems used in this
# model.
PaperRadRear = 0.3
PaperRadFront = 0.35
HTA = evalf.N(pi / 2 - pi / 10)
TrailPaper = 0.08
rake = evalf.N(-(TrailPaper*sin(HTA)-(PaperRadFront*cos(HTA))))
PaperWb = 1.02
PaperFrameCgX = 0.3
PaperFrameCgZ = 0.9
PaperForkCgX = 0.9
PaperForkCgZ = 0.7
FrameLength = evalf.N(PaperWb*sin(HTA)-(rake-(PaperRadFront-PaperRadRear)*cos(HTA)))
FrameCGNorm = evalf.N((PaperFrameCgZ - PaperRadRear-(PaperFrameCgX/sin(HTA))*cos(HTA))*sin(HTA))
FrameCGPar = evalf.N((PaperFrameCgX / sin(HTA) + (PaperFrameCgZ - PaperRadRear - PaperFrameCgX / sin(HTA) * cos(HTA)) * cos(HTA)))
tempa = evalf.N((PaperForkCgZ - PaperRadFront))
tempb = evalf.N((PaperWb-PaperForkCgX))
tempc = evalf.N(sqrt(tempa**2+tempb**2))
PaperForkL = evalf.N((PaperWb*cos(HTA)-(PaperRadFront-PaperRadRear)*sin(HTA)))
ForkCGNorm = evalf.N(rake+(tempc * sin(pi/2-HTA-acos(tempa/tempc))))
ForkCGPar = evalf.N(tempc * cos((pi/2-HTA)-acos(tempa/tempc))-PaperForkL)
# Here is the final assembly of the numerical values. The symbol 'v' is the
# forward speed of the bicycle (a concept which only makes sense in the
# upright, static equilibrium case?). These are in a dictionary which will
# later be substituted in. Again the sign on the *product* of inertia
# values is flipped here, due to different orientations of coordinate
# systems.
v = symbols('v')
val_dict = {WFrad: PaperRadFront,
WRrad: PaperRadRear,
htangle: HTA,
forkoffset: rake,
forklength: PaperForkL,
framelength: FrameLength,
forkcg1: ForkCGPar,
forkcg3: ForkCGNorm,
framecg1: FrameCGNorm,
framecg3: FrameCGPar,
Iwr11: 0.0603,
Iwr22: 0.12,
Iwf11: 0.1405,
Iwf22: 0.28,
Ifork11: 0.05892,
Ifork22: 0.06,
Ifork33: 0.00708,
Ifork31: 0.00756,
Iframe11: 9.2,
Iframe22: 11,
Iframe33: 2.8,
Iframe31: -2.4,
mfork: 4,
mframe: 85,
mwf: 3,
mwr: 2,
g: 9.81,
q1: 0,
q2: 0,
q4: 0,
q5: 0,
u1: 0,
u2: 0,
u3: v / PaperRadRear,
u4: 0,
u5: 0,
u6: v / PaperRadFront}
# Linearizes the forcing vector; the equations are set up as MM udot =
# forcing, where MM is the mass matrix, udot is the vector representing the
# time derivatives of the generalized speeds, and forcing is a vector which
# contains both external forcing terms and internal forcing terms, such as
# centripital or coriolis forces. This actually returns a matrix with as
# many rows as *total* coordinates and speeds, but only as many columns as
# independent coordinates and speeds.
forcing_lin = KM.linearize()[0]
# As mentioned above, the size of the linearized forcing terms is expanded
# to include both q's and u's, so the mass matrix must have this done as
# well. This will likely be changed to be part of the linearized process,
# for future reference.
MM_full = KM.mass_matrix_full
MM_full_s = MM_full.subs(val_dict)
forcing_lin_s = forcing_lin.subs(KM.kindiffdict()).subs(val_dict)
MM_full_s = MM_full_s.evalf()
forcing_lin_s = forcing_lin_s.evalf()
# Finally, we construct an "A" matrix for the form xdot = A x (x being the
# state vector, although in this case, the sizes are a little off). The
# following line extracts only the minimum entries required for eigenvalue
# analysis, which correspond to rows and columns for lean, steer, lean
# rate, and steer rate.
Amat = MM_full_s.inv() * forcing_lin_s
A = Amat.extract([1, 2, 4, 6], [1, 2, 3, 5])
# Precomputed for comparison
Res = Matrix([[ 0, 0, 1.0, 0],
[ 0, 0, 0, 1.0],
[9.48977444677355, -0.891197738059089*v**2 - 0.571523173729245, -0.105522449805691*v, -0.330515398992311*v],
[11.7194768719633, -1.97171508499972*v**2 + 30.9087533932407, 3.67680523332152*v, -3.08486552743311*v]])
# Actual eigenvalue comparison
eps = 1.e-12
for i in range(6):
error = Res.subs(v, i) - A.subs(v, i)
assert all(abs(x) < eps for x in error)
V = 2 * R * (pi * r)**2
rho = m / V
A = ReferenceFrame('A') # Torus fixed, x-y in symmetry plane
B = A.orientnew('B', 'Axis', [phi, A.z]) # Intermediate frame
C = B.orientnew('C', 'Axis', [-theta, B.y]) # Intermediate frame
# Position vector from torus center to arbitrary point of torus
# R : torus major radius
# s : distance >= 0 from center of torus cross section to point in torus
p = R * B.x + s * C.x
# Determinant of the Jacobian of the mapping from a, b, c to x, y, z
# See Wikipedia for a lucid explanation of why we must comput this Jacobian:
# http://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant#Further_examples
J = Matrix([dot(p, uv) for uv in A]).transpose().jacobian([phi, theta, s])
dv = J.det().trigsimp() # Need to ensure this is positive
print("dx*dy*dz = {0}*dphi*dtheta*ds".format(dv))
# We want to compute the inertia scalars of the torus relative to it's mass
# center, for the following six unit vector pairs
unit_vector_pairs = [(A.x, A.x), (A.y, A.y), (A.z, A.z), (A.x, A.y),
(A.y, A.z), (A.x, A.z)]
# Calculate the six unique inertia scalars using equation 3.3.9 of Kane &
# Levinson, 1985.
inertia_scalars = []
for n_a, n_b in unit_vector_pairs:
# Integrand of Equation 3.3.9
integrand = rho * dot(cross(p, n_a), cross(p, n_b)) * dv
m = symbols('m')
m_val = 12
N = ReferenceFrame('N')
pO = Point('O')
pBs = pO.locatenew('B*', -3 * N.x + 2 * N.y - 4 * N.z)
I_B_O = inertia(N, 260 * m / m_val, 325 * m / m_val, 169 * m / m_val,
72 * m / m_val, 96 * m / m_val, -144 * m / m_val)
print("I_B_rel_O = {0}".format(I_B_O))
I_Bs_O = inertia_of_point_mass(m, pBs.pos_from(pO), N)
print("\nI_B*_rel_O = {0}".format(I_Bs_O))
I_B_Bs = I_B_O - I_Bs_O
print("\nI_B_rel_B* = {0}".format(I_B_Bs))
pQ = pO.locatenew('Q', -4 * N.z)
I_Bs_Q = inertia_of_point_mass(m, pBs.pos_from(pQ), N)
print("\nI_B*_rel_Q = {0}".format(I_Bs_Q))
I_B_Q = I_B_Bs + I_Bs_Q
print("\nI_B_rel_Q = {0}".format(I_B_Q))
# n_a is a vector parallel to line PQ
n_a = S(3) / 5 * N.x - S(4) / 5 * N.z
I_a_a_B_Q = dot(dot(n_a, I_B_Q), n_a)
print("\nn_a = {0}".format(n_a))
print("\nI_a_a_B_Q = {0} = {1}".format(I_a_a_B_Q, I_a_a_B_Q.subs(m, m_val)))
pP1.v1pt_theory(pO, A, B)
pDs.set_vel(E, 0)
pDs.v2pt_theory(pP1, B, E)
pDs.v2pt_theory(pP1, A, E)
# X*B.z, (Y*E.y + Z*E.z) are forces the panes of glass
# exert on P1, D* respectively
R1 = X*B.z + C*E.x - m1*g*B.y
R2 = Y*E.y + Z*E.z - C*E.x - m2*g*B.y
resultants = [R1, R2]
points = [pP1, pDs]
forces = [(pP1, R1), (pDs, R2)]
system = [Particle('P1', pP1, m1), Particle('P2', pDs, m2)]
# kinematic differential equations
kde = [u1 - dot(pP1.vel(A), E.x), u2 - dot(pP1.vel(A), E.y), u3 - q3d]
kde_map = solve(kde, qd)
# include second derivatives in kde map
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
# use nonholonomic partial velocities to find the nonholonomic
# generalized active forces
vc = [dot(pDs.vel(B), E.y).subs(kde_map)]
vc_map = solve(vc, [u3])
partials = partial_velocities(points, [u1, u2], A, kde_map, vc_map)
Fr, _ = generalized_active_forces(partials, forces)
Fr_star, _ = generalized_inertia_forces(partials, system, kde_map, vc_map)
# dynamical equations
dyn_eq = [x + y for x, y in zip(Fr, Fr_star)]
from sympy import acos, pi, symbols
from sympy.physics.mechanics import ReferenceFrame, Point
from sympy.physics.mechanics import cross, dot
# vectors A, B have equal magnitude 10 N
alpha = 10 # [N]
N = ReferenceFrame('N')
pO = Point('O')
pS = pO.locatenew('S', 10*N.x + 5*N.z)
pR = pO.locatenew('R', 10*N.x + 12*N.y)
pQ = pO.locatenew('Q', 12*N.y + 10*N.z)
pP = pO.locatenew('P', 4*N.x + 7*N.z)
A = alpha * pQ.pos_from(pP).normalize()
B = alpha * pS.pos_from(pR).normalize()
R = A + B
M = cross(pP.pos_from(pO), A) + cross(pR.pos_from(pO), B)
Ms = dot(R, M) * R / dot(R, R)
ps = cross(R, M) / dot(R, R)
# Replace set S (vectors A, B) with a wrench consisting of the resultant of S,
# placed on the central axis of S and a couple whose torque is equal to S's
# moment of minimum magnitude.
r = R
C = Ms
print("|C| = {0}, {1}".format(C.magnitude(), C.magnitude().n()))
# Since the bound vector r is placed on the central axis of S, |p*| gives the
# distance from point O to r.
print("|p*| = {0}, {1}".format(ps.magnitude(), ps.magnitude().n()))
def main():
theta_zero = 30 / 180 * sp.pi #10/180 * sp.pi
bus = sun.orientnew("bus", "Axis", [angle_to_sun, sun.z])
order = [bus]
# MANUAL GENERATION FOR TWO PANELS.
'''
panel_ors["0"] = Panel(
connections="bus",
dimensions=[0.7,0.7],
frame = bus.orientnew("0", "Axis", [theta_zero, bus.z]),
state = 0
)
panel_ors["1"] = Panel(
connections="0",
dimensions=[0.7,0.7],
frame = panel_ors[0].frame.orientnew("1", "Axis", [theta_zero, panel_ors["0"].frame.z]),
state = 0
)
'''
#PANEL GENERATOR FOR LOOP.
for i in range(1, n + 1):
# for each panel, create a new reference frame.
panel_ors[i] = Panel(dimensions=[0.7, 0.7],
frame=order[i - 1].orientnew(
"panel" + str(i), "Axis",
[theta_zero, bus.z]),
state=1)
order.append(panel_ors[i].frame)
upd = 3.6**-1 # hertz
t = 1 / upd
omega_0 = 0
#return panel_ors
for panel in panel_ors:
panel = panel_ors[panel]
c = 0
alpha = 0 * panel.frame.z
theta = sp.acos(mech.dot(panel.frame.x, bus.x).evalf()) * panel.frame.z
y_axis_theta = []
y_axis_force = []
y_axis_moment_srp = []
y_axis_moment_spr = []
y_axis_moment_dam = []
y_axis_speed = []
bus_location = 1.5 * sun.x # location in AU referenced on the sun frame.
omega_0 = 0 * panel.frame.z
spring_trigger = 0
while True:
f = panel.area * (1 + panel.state) * calc_srp(bus_location, bus)
angled_force = calc_torque(panel, f, bus)
#spring = get_spring_moment(panel.frame, bus)
'''
if get_angle(panel.frame, bus) < 0:
if mech.dot(omega_0, panel.frame.z) < 0: # once it reaches a desired velocity ccw, turn off.
spring = -panel.torque * 6
else:
spring = 0 *panel.frame.z
else:
spring = 0 * panel.frame.z
'''
spring = get_spring_moment(panel.frame, bus)
damper = get_joint_damper_monent(omega_0, panel.frame)
#c = 4.29E-6 # spring constant. N*M/rad
'''
ref_spring_theta = 0 # rads
if get_angle(panel.frame, bus) < 0 and spring_trigger == 0: # start breaking
time_to_stop = 300 # seconds.
spring_coeff = - alpha.magnitude()* panel.MOI / time_to_stop
spring = -spring_coeff * (theta - ref_spring_theta)
spring_trigger = 1
elif get_angle(panel.frame, bus) > 0:
spring_trigger = 0
spring = 0 * panel.frame.z
'''
alpha = (panel.torque + spring + damper) / panel.MOI
d_theta = omega_0 * t + alpha * t * t / 2
theta += d_theta
c += 1
omega_0 = omega_0 + alpha * t
panel.frame.orient(bus, "Axis",
[mech.dot(theta, panel.frame.z), bus.z])
print(str(theta) + ", " + str(omega_0) + ", " + str(c))
y_axis_theta.append(get_angle(panel.frame, bus))
y_axis_force.append(angled_force)
y_axis_moment_spr.append(mech.dot(spring, bus.z).evalf())
y_axis_moment_srp.append(mech.dot(panel.torque, bus.z).evalf())
y_axis_moment_dam.append(mech.dot(damper, bus.z).evalf())
y_axis_speed.append(mech.dot(omega_0, bus.z).evalf())
if c > 2000:
print("Iteration limit reached, escaping...")
break
if c > 1 and threshold(np.mean(
np.abs(np.diff(y_axis_theta[-convergence_mean:]))),
0,
thresh=0.0001):
print("Solution found after " + str(c) + " iterations")
print("Force: " + "%.3g" % (2 * n * y_axis_force[-1]) + "N")
break
if abs(get_angle(panel.frame, bus)) > sp.pi.evalf() / 2:
print("Exception ocurred: Panel rotated more than 90 degrees.")
print("Force: " + "%.3g" % (2 * n * y_axis_force[-1]) + "N")
break
x_axis = np.linspace(0.0, c * t, c)
figtheta, axtheta = plt.subplots() # Create a figure and an axes.
axtheta.plot(x_axis, y_axis_theta)
axtheta.plot([0, c * t], [y_axis_theta[-1], y_axis_theta[-1]], "r--")
axtheta.set_xlabel('Time (s)') # Add an x-label to the axes.
axtheta.set_ylabel('Panel angle (rads)') # Add a y-label to the axes.
axtheta.set_title(
"Angle plot against time, initial theta =" +
str(round(theta_zero.evalf(), 2))) # Add a title to the axes.
axtheta.grid(True)
#figtheta.savefig("theta_plot.fig")
figtheta.savefig("theta_plot.png")
#axtheta.grid(axis="both", linestyle='-', linewidth=2)
figforce, axforce = plt.subplots()
axforce.plot(x_axis, y_axis_force)
axforce.plot([0, c * t], [y_axis_force[-1], y_axis_force[-1]], "r--")
axforce.set_xlabel("Time (s)")
axforce.set_ylabel("SRP Force (N)")
axforce.set_title("Force plot against time, initial theta =" +
str(round(theta_zero.evalf(), 2)))
axforce.grid(True)
#axforce.savefig("force_plot.fig")
figforce.savefig("force_plot.png")
#axforce.grid(axis="both", linestyle='-', linewidth=2)
figmoment, axmoment = plt.subplots()
axmoment.plot(x_axis, y_axis_moment_spr, label="Spring moment")
axmoment.plot(x_axis, y_axis_moment_srp, label="SRP moment")
axmoment.plot(x_axis, y_axis_moment_dam, label="Damper moment")
axmoment.plot([0, c * t], [
np.mean(y_axis_moment_srp[-convergence_mean:]),
np.mean(y_axis_moment_srp[-convergence_mean:])
], "r--")
axmoment.set_xlabel("Time (s)")
axmoment.set_ylabel("Moment (N*m)")
axmoment.set_title("Moment plot against time, initial theta =" +
str(round(theta_zero.evalf(), 2)))
axmoment.legend()
axmoment.grid(True)
#axmoment.savefig("moment_plot.fig")
figmoment.savefig("moment_plot.png")
#axmoment.grid(axis="both", linestyle='-', linewidth=2)
#
figspeed, axspeed = plt.subplots()
axspeed.plot(x_axis, y_axis_speed)
axspeed.plot([0, c * t], [0, 0], "r--")
axspeed.set_xlabel("Time (s)")
axspeed.set_ylabel("Angular velocity (Rads/s)")
axspeed.set_title(
"Angular velocity plot against time, initial theta =" +
str(round(theta_zero.evalf(), 2)))
axspeed.grid(True)
#axspeed.savefig("speed_plot.fig")
figspeed.savefig("speed_plot.png")
#axspeed.grid(axis="both", linestyle='-', linewidth=2)
#plt.show()
return panel_ors, bus
# C^ is the point in disk C that comes into contact with plane H.
pC_hat = pR.locatenew("C^", 0)
pC_hat.set_vel(C, 0)
# C* is the point at the center of disk C.
pC_star = pC_hat.locatenew("C*", R * B.y)
pC_star.set_vel(C, 0)
pC_star.set_vel(B, 0)
# calculate velocities in A
pC_star.v2pt_theory(pR, A, B)
pC_hat.v2pt_theory(pC_star, A, C)
# kinematic differential equations
kde = [x - y for x, y in zip([dot(C.ang_vel_in(A), basis) for basis in B] + qd[3:], u)]
kde_map = solve(kde, qd)
# include second derivatives in kde map
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
vc = map(lambda x: dot(pC_hat.vel(A), x), [A.x, A.y])
vc_map = solve(subs(vc, kde_map), [u4, u5])
# define disc rigidbody
I_C = inertia(C, m * R ** 2 / 4, m * R ** 2 / 4, m * R ** 2 / 2)
rbC = RigidBody("rbC", pC_star, C, m, (I_C, pC_star))
# forces
R_C_hat = Px * A.x + Py * A.y + Pz * A.z
R_C_star = -m * g * A.z
# No Input test case
initial_altitude=10
numSteps=10
timeStep=1
param1=[1]*len(param)
pos1=[0]*len(state)
vel1=[0]*len(velocity)
pos1[1]=initial_altitude
input1=[0]*4
NoInput=QuadcopterMotion(param1,vel1+pos1)
for i in range(numSteps):
NoInput.move(input1,timeStep)
#print NoInput.state
from sympy import simplify
from sympy.physics.mechanics import dot
gC=dot(NewtonFrame.y,RobotFrame.x),dot(NewtonFrame.y,RobotFrame.y),dot(NewtonFrame.y,RobotFrame.z)
Frame=[RobotFrame.x,RobotFrame.y,RobotFrame.z]
alpha=Symbol('alpha')
#print([simplify(dot(RobotFrame.ang_vel_in(NewtonFrame),vec).subs(symdict)) for vec in Frame])
#print([simplify(dot(NewtonFrame.y,vec).subs(symdict)) for vec in Frame])
#print([simplify(dot(cos(alpha)*NewtonFrame.x-sin(alpha)*NewtonFrame.y,vec).subs(symdict)) for vec in Frame])
try:
from quadcopterODE import quadcopterODE
except ImportError:
from warnings import warn
warn("quadcopterODE not written yet; run QuadcopterMechanics.py")
from scipy.integrate import ode
class QuadcopterMotion:
def __init__(self,param,init_state):
self.param=param
self.ODE=ode(quadcopterODE)#.set_integrator('dopri5')
def test_Vector():
assert A.x != A.y
assert A.y != A.z
assert A.z != A.x
v1 = x*A.x + y*A.y + z*A.z
v2 = x**2*A.x + y**2*A.y + z**2*A.z
v3 = v1 + v2
v4 = v1 - v2
assert isinstance(v1, Vector)
assert dot(v1, A.x) == x
assert dot(v1, A.y) == y
assert dot(v1, A.z) == z
assert isinstance(v2, Vector)
assert dot(v2, A.x) == x**2
assert dot(v2, A.y) == y**2
assert dot(v2, A.z) == z**2
assert isinstance(v3, Vector)
# We probably shouldn't be using simplify in dot...
assert dot(v3, A.x) == (x*(x + 1)).expand()
assert dot(v3, A.y) == (y*(y + 1)).expand()
assert dot(v3, A.z) == (z*(z + 1)).expand()
assert isinstance(v4, Vector)
# We probably shouldn't be using simplify in dot...
assert dot(v4, A.x) == (x*(1 - x)).expand()
assert dot(v4, A.y) == (y*(1 - y)).expand()
assert dot(v4, A.z) == (z*(1 - z)).expand()
pC_star = pD.locatenew('C*', -b*A.z)
for p in [pA_star, pB_star, pC_star]:
p.set_vel(A, 0)
p.v2pt_theory(pD, F, A)
# points of B, C touching the plane P
pB_hat = pB_star.locatenew('B^', -R*A.x)
pC_hat = pC_star.locatenew('C^', -R*A.x)
pB_hat.set_vel(B, 0)
pC_hat.set_vel(C, 0)
pB_hat.v2pt_theory(pB_star, F, B)
pC_hat.v2pt_theory(pC_star, F, C)
# the velocities of B^, C^ are zero since B, C are assumed to roll without slip
#kde = [dot(p.vel(F), b) for b in A for p in [pB_hat, pC_hat]]
kde = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]
kde_map = solve(kde, [q2d, q3d])
# need to add q2'', q3'' terms manually since subs does not replace
# Derivative(q(t), t, t) with Derivative(Derivative(q(t), t))
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
# inertias of bodies A, B, C
# IA22, IA23, IA33 are not specified in the problem statement, but are
# necessary to define an inertia object. Although the values of
# IA22, IA23, IA33 are not known in terms of the variables given in the
# problem statement, they do not appear in the general inertia terms.
inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
inertia_B = inertia(B, K, K, J)
inertia_C = inertia(C, K, K, J)
def test_sub_qdot():
# This test solves exercises 8.12, 8.17 from Kane 1985 and defines
# some velocities in terms of q, qdot.
## --- Declare symbols ---
q1, q2, q3 = dynamicsymbols('q1:4')
q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
u1, u2, u3 = dynamicsymbols('u1:4')
u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta')
a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
Q1, Q2, Q3 = symbols('Q1 Q2 Q3')
# --- Reference Frames ---
F = ReferenceFrame('F')
P = F.orientnew('P', 'axis', [-theta, F.y])
A = P.orientnew('A', 'axis', [q1, P.x])
A.set_ang_vel(F, u1*A.x + u3*A.z)
# define frames for wheels
B = A.orientnew('B', 'axis', [q2, A.z])
C = A.orientnew('C', 'axis', [q3, A.z])
## --- define points D, S*, Q on frame A and their velocities ---
pD = Point('D')
pD.set_vel(A, 0)
# u3 will not change v_D_F since wheels are still assumed to roll w/o slip
pD.set_vel(F, u2 * A.y)
pS_star = pD.locatenew('S*', e*A.y)
pQ = pD.locatenew('Q', f*A.y - R*A.x)
# masscenters of bodies A, B, C
pA_star = pD.locatenew('A*', a*A.y)
pB_star = pD.locatenew('B*', b*A.z)
pC_star = pD.locatenew('C*', -b*A.z)
for p in [pS_star, pQ, pA_star, pB_star, pC_star]:
p.v2pt_theory(pD, F, A)
# points of B, C touching the plane P
pB_hat = pB_star.locatenew('B^', -R*A.x)
pC_hat = pC_star.locatenew('C^', -R*A.x)
pB_hat.v2pt_theory(pB_star, F, B)
pC_hat.v2pt_theory(pC_star, F, C)
# --- relate qdot, u ---
# the velocities of B^, C^ are zero since B, C are assumed to roll w/o slip
kde = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]
kde += [u1 - q1d]
kde_map = solve(kde, [q1d, q2d, q3d])
for k, v in list(kde_map.items()):
kde_map[k.diff(t)] = v.diff(t)
# inertias of bodies A, B, C
# IA22, IA23, IA33 are not specified in the problem statement, but are
# necessary to define an inertia object. Although the values of
# IA22, IA23, IA33 are not known in terms of the variables given in the
# problem statement, they do not appear in the general inertia terms.
inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
inertia_B = inertia(B, K, K, J)
inertia_C = inertia(C, K, K, J)
# define the rigid bodies A, B, C
rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
## --- use kanes method ---
km = KanesMethod(F, [q1, q2, q3], [u1, u2], kd_eqs=kde, u_auxiliary=[u3])
forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x + Q2*A.y + Q3*A.z)]
bodies = [rbA, rbB, rbC]
# Q2 = -u_prime * u2 * Q1 / sqrt(u2**2 + f**2 * u1**2)
# -u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2) = R / Q1 * Q2
fr_expected = Matrix([
f*Q3 + M*g*e*sin(theta)*cos(q1),
Q2 + M*g*sin(theta)*sin(q1),
e*M*g*cos(theta) - Q1*f - Q2*R])
#Q1 * (f - u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2)))])
fr_star_expected = Matrix([
-(IA + 2*J*b**2/R**2 + 2*K +
mA*a**2 + 2*mB*b**2) * u1.diff(t) - mA*a*u1*u2,
-(mA + 2*mB +2*J/R**2) * u2.diff(t) + mA*a*u1**2,
0])
fr, fr_star = km.kanes_equations(forces, bodies)
assert (fr.expand() == fr_expected.expand())
assert (trigsimp(fr_star).expand() == fr_star_expected.expand())
def test_bicycle():
if ON_TRAVIS:
skip("Too slow for travis.")
# Code to get equations of motion for a bicycle modeled as in:
# J.P Meijaard, Jim M Papadopoulos, Andy Ruina and A.L Schwab. Linearized
# dynamics equations for the balance and steer of a bicycle: a benchmark
# and review. Proceedings of The Royal Society (2007) 463, 1955-1982
# doi: 10.1098/rspa.2007.1857
# Note that this code has been crudely ported from Autolev, which is the
# reason for some of the unusual naming conventions. It was purposefully as
# similar as possible in order to aide debugging.
# Declare Coordinates & Speeds
# Simple definitions for qdots - qd = u
# Speeds are: yaw frame ang. rate, roll frame ang. rate, rear wheel frame
# ang. rate (spinning motion), frame ang. rate (pitching motion), steering
# frame ang. rate, and front wheel ang. rate (spinning motion).
# Wheel positions are ignorable coordinates, so they are not introduced.
q1, q2, q4, q5 = dynamicsymbols('q1 q2 q4 q5')
q1d, q2d, q4d, q5d = dynamicsymbols('q1 q2 q4 q5', 1)
u1, u2, u3, u4, u5, u6 = dynamicsymbols('u1 u2 u3 u4 u5 u6')
u1d, u2d, u3d, u4d, u5d, u6d = dynamicsymbols('u1 u2 u3 u4 u5 u6', 1)
# Declare System's Parameters
WFrad, WRrad, htangle, forkoffset = symbols(
'WFrad WRrad htangle forkoffset')
forklength, framelength, forkcg1 = symbols(
'forklength framelength forkcg1')
forkcg3, framecg1, framecg3, Iwr11 = symbols(
'forkcg3 framecg1 framecg3 Iwr11')
Iwr22, Iwf11, Iwf22, Iframe11 = symbols('Iwr22 Iwf11 Iwf22 Iframe11')
Iframe22, Iframe33, Iframe31, Ifork11 = symbols(
'Iframe22 Iframe33 Iframe31 Ifork11')
Ifork22, Ifork33, Ifork31, g = symbols('Ifork22 Ifork33 Ifork31 g')
mframe, mfork, mwf, mwr = symbols('mframe mfork mwf mwr')
# Set up reference frames for the system
# N - inertial
# Y - yaw
# R - roll
# WR - rear wheel, rotation angle is ignorable coordinate so not oriented
# Frame - bicycle frame
# TempFrame - statically rotated frame for easier reference inertia definition
# Fork - bicycle fork
# TempFork - statically rotated frame for easier reference inertia definition
# WF - front wheel, again posses a ignorable coordinate
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Axis', [q1, N.z])
R = Y.orientnew('R', 'Axis', [q2, Y.x])
Frame = R.orientnew('Frame', 'Axis', [q4 + htangle, R.y])
WR = ReferenceFrame('WR')
TempFrame = Frame.orientnew('TempFrame', 'Axis', [-htangle, Frame.y])
Fork = Frame.orientnew('Fork', 'Axis', [q5, Frame.x])
TempFork = Fork.orientnew('TempFork', 'Axis', [-htangle, Fork.y])
WF = ReferenceFrame('WF')
# Kinematics of the Bicycle First block of code is forming the positions of
# the relevant points
# rear wheel contact -> rear wheel mass center -> frame mass center +
# frame/fork connection -> fork mass center + front wheel mass center ->
# front wheel contact point
WR_cont = Point('WR_cont')
WR_mc = WR_cont.locatenew('WR_mc', WRrad * R.z)
Steer = WR_mc.locatenew('Steer', framelength * Frame.z)
Frame_mc = WR_mc.locatenew('Frame_mc',
-framecg1 * Frame.x + framecg3 * Frame.z)
Fork_mc = Steer.locatenew('Fork_mc', -forkcg1 * Fork.x + forkcg3 * Fork.z)
WF_mc = Steer.locatenew('WF_mc', forklength * Fork.x + forkoffset * Fork.z)
WF_cont = WF_mc.locatenew(
'WF_cont',
WFrad * (dot(Fork.y, Y.z) * Fork.y - Y.z).normalize())
# Set the angular velocity of each frame.
# Angular accelerations end up being calculated automatically by
# differentiating the angular velocities when first needed.
# u1 is yaw rate
# u2 is roll rate
# u3 is rear wheel rate
# u4 is frame pitch rate
# u5 is fork steer rate
# u6 is front wheel rate
Y.set_ang_vel(N, u1 * Y.z)
R.set_ang_vel(Y, u2 * R.x)
WR.set_ang_vel(Frame, u3 * Frame.y)
Frame.set_ang_vel(R, u4 * Frame.y)
Fork.set_ang_vel(Frame, u5 * Fork.x)
WF.set_ang_vel(Fork, u6 * Fork.y)
# Form the velocities of the previously defined points, using the 2 - point
# theorem (written out by hand here). Accelerations again are calculated
# automatically when first needed.
WR_cont.set_vel(N, 0)
WR_mc.v2pt_theory(WR_cont, N, WR)
Steer.v2pt_theory(WR_mc, N, Frame)
Frame_mc.v2pt_theory(WR_mc, N, Frame)
Fork_mc.v2pt_theory(Steer, N, Fork)
WF_mc.v2pt_theory(Steer, N, Fork)
WF_cont.v2pt_theory(WF_mc, N, WF)
# Sets the inertias of each body. Uses the inertia frame to construct the
# inertia dyadics. Wheel inertias are only defined by principle moments of
# inertia, and are in fact constant in the frame and fork reference frames;
# it is for this reason that the orientations of the wheels does not need
# to be defined. The frame and fork inertias are defined in the 'Temp'
# frames which are fixed to the appropriate body frames; this is to allow
# easier input of the reference values of the benchmark paper. Note that
# due to slightly different orientations, the products of inertia need to
# have their signs flipped; this is done later when entering the numerical
# value.
Frame_I = (inertia(TempFrame, Iframe11, Iframe22, Iframe33, 0, 0,
Iframe31), Frame_mc)
Fork_I = (inertia(TempFork, Ifork11, Ifork22, Ifork33, 0, 0,
Ifork31), Fork_mc)
WR_I = (inertia(Frame, Iwr11, Iwr22, Iwr11), WR_mc)
WF_I = (inertia(Fork, Iwf11, Iwf22, Iwf11), WF_mc)
# Declaration of the RigidBody containers. ::
BodyFrame = RigidBody('BodyFrame', Frame_mc, Frame, mframe, Frame_I)
BodyFork = RigidBody('BodyFork', Fork_mc, Fork, mfork, Fork_I)
BodyWR = RigidBody('BodyWR', WR_mc, WR, mwr, WR_I)
BodyWF = RigidBody('BodyWF', WF_mc, WF, mwf, WF_I)
# The kinematic differential equations; they are defined quite simply. Each
# entry in this list is equal to zero.
kd = [q1d - u1, q2d - u2, q4d - u4, q5d - u5]
# The nonholonomic constraints are the velocity of the front wheel contact
# point dotted into the X, Y, and Z directions; the yaw frame is used as it
# is "closer" to the front wheel (1 less DCM connecting them). These
# constraints force the velocity of the front wheel contact point to be 0
# in the inertial frame; the X and Y direction constraints enforce a
# "no-slip" condition, and the Z direction constraint forces the front
# wheel contact point to not move away from the ground frame, essentially
# replicating the holonomic constraint which does not allow the frame pitch
# to change in an invalid fashion.
conlist_speed = [
WF_cont.vel(N) & Y.x,
WF_cont.vel(N) & Y.y,
WF_cont.vel(N) & Y.z
]
# The holonomic constraint is that the position from the rear wheel contact
# point to the front wheel contact point when dotted into the
# normal-to-ground plane direction must be zero; effectively that the front
# and rear wheel contact points are always touching the ground plane. This
# is actually not part of the dynamic equations, but instead is necessary
# for the lineraization process.
conlist_coord = [WF_cont.pos_from(WR_cont) & Y.z]
# The force list; each body has the appropriate gravitational force applied
# at its mass center.
FL = [(Frame_mc, -mframe * g * Y.z), (Fork_mc, -mfork * g * Y.z),
(WF_mc, -mwf * g * Y.z), (WR_mc, -mwr * g * Y.z)]
BL = [BodyFrame, BodyFork, BodyWR, BodyWF]
# The N frame is the inertial frame, coordinates are supplied in the order
# of independent, dependent coordinates, as are the speeds. The kinematic
# differential equation are also entered here. Here the dependent speeds
# are specified, in the same order they were provided in earlier, along
# with the non-holonomic constraints. The dependent coordinate is also
# provided, with the holonomic constraint. Again, this is only provided
# for the linearization process.
KM = KanesMethod(N,
q_ind=[q1, q2, q5],
q_dependent=[q4],
configuration_constraints=conlist_coord,
u_ind=[u2, u3, u5],
u_dependent=[u1, u4, u6],
velocity_constraints=conlist_speed,
kd_eqs=kd)
(fr, frstar) = KM.kanes_equations(FL, BL)
# This is the start of entering in the numerical values from the benchmark
# paper to validate the eigen values of the linearized equations from this
# model to the reference eigen values. Look at the aforementioned paper for
# more information. Some of these are intermediate values, used to
# transform values from the paper into the coordinate systems used in this
# model.
PaperRadRear = 0.3
PaperRadFront = 0.35
HTA = evalf.N(pi / 2 - pi / 10)
TrailPaper = 0.08
rake = evalf.N(-(TrailPaper * sin(HTA) - (PaperRadFront * cos(HTA))))
PaperWb = 1.02
PaperFrameCgX = 0.3
PaperFrameCgZ = 0.9
PaperForkCgX = 0.9
PaperForkCgZ = 0.7
FrameLength = evalf.N(PaperWb * sin(HTA) -
(rake - (PaperRadFront - PaperRadRear) * cos(HTA)))
FrameCGNorm = evalf.N((PaperFrameCgZ - PaperRadRear -
(PaperFrameCgX / sin(HTA)) * cos(HTA)) * sin(HTA))
FrameCGPar = evalf.N(
(PaperFrameCgX / sin(HTA) +
(PaperFrameCgZ - PaperRadRear - PaperFrameCgX / sin(HTA) * cos(HTA)) *
cos(HTA)))
tempa = evalf.N((PaperForkCgZ - PaperRadFront))
tempb = evalf.N((PaperWb - PaperForkCgX))
tempc = evalf.N(sqrt(tempa**2 + tempb**2))
PaperForkL = evalf.N(
(PaperWb * cos(HTA) - (PaperRadFront - PaperRadRear) * sin(HTA)))
ForkCGNorm = evalf.N(rake +
(tempc * sin(pi / 2 - HTA - acos(tempa / tempc))))
ForkCGPar = evalf.N(tempc * cos((pi / 2 - HTA) - acos(tempa / tempc)) -
PaperForkL)
# Here is the final assembly of the numerical values. The symbol 'v' is the
# forward speed of the bicycle (a concept which only makes sense in the
# upright, static equilibrium case?). These are in a dictionary which will
# later be substituted in. Again the sign on the *product* of inertia
# values is flipped here, due to different orientations of coordinate
# systems.
v = symbols('v')
val_dict = {
WFrad: PaperRadFront,
WRrad: PaperRadRear,
htangle: HTA,
forkoffset: rake,
forklength: PaperForkL,
framelength: FrameLength,
forkcg1: ForkCGPar,
forkcg3: ForkCGNorm,
framecg1: FrameCGNorm,
framecg3: FrameCGPar,
Iwr11: 0.0603,
Iwr22: 0.12,
Iwf11: 0.1405,
Iwf22: 0.28,
Ifork11: 0.05892,
Ifork22: 0.06,
Ifork33: 0.00708,
Ifork31: 0.00756,
Iframe11: 9.2,
Iframe22: 11,
Iframe33: 2.8,
Iframe31: -2.4,
mfork: 4,
mframe: 85,
mwf: 3,
mwr: 2,
g: 9.81,
q1: 0,
q2: 0,
q4: 0,
q5: 0,
u1: 0,
u2: 0,
u3: v / PaperRadRear,
u4: 0,
u5: 0,
u6: v / PaperRadFront
}
# Linearizes the forcing vector; the equations are set up as MM udot =
# forcing, where MM is the mass matrix, udot is the vector representing the
# time derivatives of the generalized speeds, and forcing is a vector which
# contains both external forcing terms and internal forcing terms, such as
# centripital or coriolis forces. This actually returns a matrix with as
# many rows as *total* coordinates and speeds, but only as many columns as
# independent coordinates and speeds.
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
forcing_lin = KM.linearize()[0]
# As mentioned above, the size of the linearized forcing terms is expanded
# to include both q's and u's, so the mass matrix must have this done as
# well. This will likely be changed to be part of the linearized process,
# for future reference.
MM_full = KM.mass_matrix_full
MM_full_s = MM_full.subs(val_dict)
forcing_lin_s = forcing_lin.subs(KM.kindiffdict()).subs(val_dict)
MM_full_s = MM_full_s.evalf()
forcing_lin_s = forcing_lin_s.evalf()
# Finally, we construct an "A" matrix for the form xdot = A x (x being the
# state vector, although in this case, the sizes are a little off). The
# following line extracts only the minimum entries required for eigenvalue
# analysis, which correspond to rows and columns for lean, steer, lean
# rate, and steer rate.
Amat = MM_full_s.inv() * forcing_lin_s
A = Amat.extract([1, 2, 4, 6], [1, 2, 3, 5])
# Precomputed for comparison
Res = Matrix([[0, 0, 1.0, 0], [0, 0, 0, 1.0],
[
9.48977444677355,
-0.891197738059089 * v**2 - 0.571523173729245,
-0.105522449805691 * v, -0.330515398992311 * v
],
[
11.7194768719633,
-1.97171508499972 * v**2 + 30.9087533932407,
3.67680523332152 * v, -3.08486552743311 * v
]])
# Actual eigenvalue comparison
eps = 1.e-12
for i in xrange(6):
error = Res.subs(v, i) - A.subs(v, i)
assert all(abs(x) < eps for x in error)
b1, b2, b3 = symbols('b1 b2 b3', real=True)
p1, p2, p3 = symbols('p1 p2 p3', real=True)
N = ReferenceFrame('N')
pO = Point('O')
pS = pO.locatenew('S', 10*N.x + 5*N.z)
pR = pO.locatenew('R', 10*N.x + 12*N.y)
pQ = pO.locatenew('Q', 12*N.y + 10*N.z)
pP = pO.locatenew('P', 4*N.x + 7*N.z)
A = alpha * pQ.pos_from(pP).normalize()
B = alpha * pS.pos_from(pR).normalize()
R = A + B
M = cross(pP.pos_from(pO), A) + cross(pR.pos_from(pO), B)
Ms = dot(R, M) * R / dot(R, R)
ps = cross(R, M) / dot(R, R)
A_prime = beta * pP.pos_from(pO).normalize()
B_prime = b1*N.x + b2*N.y + b3*N.z
pB_prime = pO.locatenew("B'", p1*N.x + p2*N.y + p3*N.z)
M_prime = cross(pB_prime.pos_from(pO), B_prime)
eqs = [dot(R - A_prime - B_prime, n) for n in N]
eqs += [dot(M - M_prime, n) for n in N]
# choose pB_prime to be perpendicular to B_prime
# then pB_prime.magnitude() gives the distance d from O
# to the line of action of B'
eqs.append(dot(pB_prime.pos_from(pO), B_prime))
# C* is the point at the center of disk C.
pC_star = pC_hat.locatenew('C*', R*B.y)
pC_star.set_vel(C, 0)
pC_star.set_vel(B, 0)
# calculate velocities in A
pC_star.v2pt_theory(pR, A, B)
pC_hat.v2pt_theory(pC_star, A, C)
# kinematic differential equations
#kde = [dot(C.ang_vel_in(A), x) - y for x, y in zip(B, u[:3])]
#kde += [x - y for x, y in zip(qd[3:], u[3:])]
#kde_map = solve(kde, qd)
kde = [x - y for x, y in zip(u, qd)]
kde_map = solve(kde, qd)
vc = map(lambda x: dot(pC_hat.vel(A), x), [A.x, A.y])
vc_map = solve(subs(vc, kde_map), [u4, u5])
# define disc rigidbody
IC = inertia(C, m*R**2/4, m*R**2/4, m*R**2/2)
rbC = RigidBody('rbC', pC_star, C, m, (IC, pC_star))
rbC.set_potential_energy(m*g*dot(pC_star.pos_from(pR), A.z))
# potential energy
V = rbC.potential_energy
print('V = {0}'.format(msprint(V)))
# kinetic energy
K = trigsimp(rbC.kinetic_energy(A).subs(kde_map).subs(vc_map))
print('K = {0}'.format(msprint(K)))
pP1.v1pt_theory(pO, A, B)
pDs.set_vel(E, 0)
pDs.v2pt_theory(pP1, B, E)
pDs.v2pt_theory(pP1, A, E)
# X*B.z, (Y*E.y + Z*E.z) are forces the panes of glass
# exert on P1, D* respectively
R1 = X * B.z + C * E.x - m1 * g * B.y
R2 = Y * E.y + Z * E.z - C * E.x - m2 * g * B.y
resultants = [R1, R2]
points = [pP1, pDs]
forces = [(pP1, R1), (pDs, R2)]
system = [Particle('P1', pP1, m1), Particle('P2', pDs, m2)]
# kinematic differential equations
kde = [u1 - dot(pP1.vel(A), E.x), u2 - dot(pP1.vel(A), E.y), u3 - q3d]
kde_map = solve(kde, qd)
# include second derivatives in kde map
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
# use nonholonomic partial velocities to find the nonholonomic
# generalized active forces
vc = [dot(pDs.vel(B), E.y).subs(kde_map)]
vc_map = solve(vc, [u3])
partials = partial_velocities(points, [u1, u2], A, kde_map, vc_map)
Fr, _ = generalized_active_forces(partials, forces)
Fr_star, _ = generalized_inertia_forces(partials, system, kde_map, vc_map)
# dynamical equations
dyn_eq = [x + y for x, y in zip(Fr, Fr_star)]
def test_aux_dep():
# This test is about rolling disc dynamics, comparing the results found
# with KanesMethod to those found when deriving the equations "manually"
# with SymPy.
# The terms Fr, Fr*, and Fr*_steady are all compared between the two
# methods. Here, Fr*_steady refers to the generalized inertia forces for an
# equilibrium configuration.
# Note: comparing to the test of test_rolling_disc() in test_kane.py, this
# test also tests auxiliary speeds and configuration and motion constraints
#, seen in the generalized dependent coordinates q[3], and depend speeds
# u[3], u[4] and u[5].
# First, mannual derivation of Fr, Fr_star, Fr_star_steady.
# Symbols for time and constant parameters.
# Symbols for contact forces: Fx, Fy, Fz.
t, r, m, g, I, J = symbols('t r m g I J')
Fx, Fy, Fz = symbols('Fx Fy Fz')
# Configuration variables and their time derivatives:
# q[0] -- yaw
# q[1] -- lean
# q[2] -- spin
# q[3] -- dot(-r*B.z, A.z) -- distance from ground plane to disc center in
# A.z direction
# Generalized speeds and their time derivatives:
# u[0] -- disc angular velocity component, disc fixed x direction
# u[1] -- disc angular velocity component, disc fixed y direction
# u[2] -- disc angular velocity component, disc fixed z direction
# u[3] -- disc velocity component, A.x direction
# u[4] -- disc velocity component, A.y direction
# u[5] -- disc velocity component, A.z direction
# Auxiliary generalized speeds:
# ua[0] -- contact point auxiliary generalized speed, A.x direction
# ua[1] -- contact point auxiliary generalized speed, A.y direction
# ua[2] -- contact point auxiliary generalized speed, A.z direction
q = dynamicsymbols('q:4')
qd = [qi.diff(t) for qi in q]
u = dynamicsymbols('u:6')
ud = [ui.diff(t) for ui in u]
#ud_zero = {udi : 0 for udi in ud}
ud_zero = dict(zip(ud, [0.]*len(ud)))
ua = dynamicsymbols('ua:3')
#ua_zero = {uai : 0 for uai in ua}
ua_zero = dict(zip(ua, [0.]*len(ua)))
# Reference frames:
# Yaw intermediate frame: A.
# Lean intermediate frame: B.
# Disc fixed frame: C.
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q[0], N.z])
B = A.orientnew('B', 'Axis', [q[1], A.x])
C = B.orientnew('C', 'Axis', [q[2], B.y])
# Angular velocity and angular acceleration of disc fixed frame
# u[0], u[1] and u[2] are generalized independent speeds.
C.set_ang_vel(N, u[0]*B.x + u[1]*B.y + u[2]*B.z)
C.set_ang_acc(N, C.ang_vel_in(N).diff(t, B)
+ cross(B.ang_vel_in(N), C.ang_vel_in(N)))
# Velocity and acceleration of points:
# Disc-ground contact point: P.
# Center of disc: O, defined from point P with depend coordinate: q[3]
# u[3], u[4] and u[5] are generalized dependent speeds.
P = Point('P')
P.set_vel(N, ua[0]*A.x + ua[1]*A.y + ua[2]*A.z)
O = P.locatenew('O', q[3]*A.z + r*sin(q[1])*A.y)
O.set_vel(N, u[3]*A.x + u[4]*A.y + u[5]*A.z)
O.set_acc(N, O.vel(N).diff(t, A) + cross(A.ang_vel_in(N), O.vel(N)))
# Kinematic differential equations:
# Two equalities: one is w_c_n_qd = C.ang_vel_in(N) in three coordinates
# directions of B, for qd0, qd1 and qd2.
# the other is v_o_n_qd = O.vel(N) in A.z direction for qd3.
# Then, solve for dq/dt's in terms of u's: qd_kd.
w_c_n_qd = qd[0]*A.z + qd[1]*B.x + qd[2]*B.y
v_o_n_qd = O.pos_from(P).diff(t, A) + cross(A.ang_vel_in(N), O.pos_from(P))
kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
[dot(v_o_n_qd - O.vel(N), A.z)])
qd_kd = solve(kindiffs, qd)
# Values of generalized speeds during a steady turn for later substitution
# into the Fr_star_steady.
steady_conditions = solve(kindiffs.subs({qd[1] : 0, qd[3] : 0}), u)
steady_conditions.update({qd[1] : 0, qd[3] : 0})
# Partial angular velocities and velocities.
partial_w_C = [C.ang_vel_in(N).diff(ui, N) for ui in u + ua]
partial_v_O = [O.vel(N).diff(ui, N) for ui in u + ua]
partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua]
# Configuration constraint: f_c, the projection of radius r in A.z direction
# is q[3].
# Velocity constraints: f_v, for u3, u4 and u5.
# Acceleration constraints: f_a.
f_c = Matrix([dot(-r*B.z, A.z) - q[3]])
f_v = Matrix([dot(O.vel(N) - (P.vel(N) + cross(C.ang_vel_in(N),
O.pos_from(P))), ai).expand() for ai in A])
v_o_n = cross(C.ang_vel_in(N), O.pos_from(P))
a_o_n = v_o_n.diff(t, A) + cross(A.ang_vel_in(N), v_o_n)
f_a = Matrix([dot(O.acc(N) - a_o_n, ai) for ai in A])
# Solve for constraint equations in the form of
# u_dependent = A_rs * [u_i; u_aux].
# First, obtain constraint coefficient matrix: M_v * [u; ua] = 0;
# Second, taking u[0], u[1], u[2] as independent,
# taking u[3], u[4], u[5] as dependent,
# rearranging the matrix of M_v to be A_rs for u_dependent.
# Third, u_aux ==0 for u_dep, and resulting dictionary of u_dep_dict.
M_v = zeros(3, 9)
for i in range(3):
for j, ui in enumerate(u + ua):
M_v[i, j] = f_v[i].diff(ui)
M_v_i = M_v[:, :3]
M_v_d = M_v[:, 3:6]
M_v_aux = M_v[:, 6:]
M_v_i_aux = M_v_i.row_join(M_v_aux)
A_rs = - M_v_d.inv() * M_v_i_aux
u_dep = A_rs[:, :3] * Matrix(u[:3])
u_dep_dict = dict(zip(u[3:], u_dep))
#u_dep_dict = {udi : u_depi[0] for udi, u_depi in zip(u[3:], u_dep.tolist())}
# Active forces: F_O acting on point O; F_P acting on point P.
# Generalized active forces (unconstrained): Fr_u = F_point * pv_point.
F_O = m*g*A.z
F_P = Fx * A.x + Fy * A.y + Fz * A.z
Fr_u = Matrix([dot(F_O, pv_o) + dot(F_P, pv_p) for pv_o, pv_p in
zip(partial_v_O, partial_v_P)])
# Inertia force: R_star_O.
# Inertia of disc: I_C_O, where J is a inertia component about principal axis.
# Inertia torque: T_star_C.
# Generalized inertia forces (unconstrained): Fr_star_u.
R_star_O = -m*O.acc(N)
I_C_O = inertia(B, I, J, I)
T_star_C = -(dot(I_C_O, C.ang_acc_in(N)) \
+ cross(C.ang_vel_in(N), dot(I_C_O, C.ang_vel_in(N))))
Fr_star_u = Matrix([dot(R_star_O, pv) + dot(T_star_C, pav) for pv, pav in
zip(partial_v_O, partial_w_C)])
# Form nonholonomic Fr: Fr_c, and nonholonomic Fr_star: Fr_star_c.
# Also, nonholonomic Fr_star in steady turning condition: Fr_star_steady.
Fr_c = Fr_u[:3, :].col_join(Fr_u[6:, :]) + A_rs.T * Fr_u[3:6, :]
Fr_star_c = Fr_star_u[:3, :].col_join(Fr_star_u[6:, :])\
+ A_rs.T * Fr_star_u[3:6, :]
Fr_star_steady = Fr_star_c.subs(ud_zero).subs(u_dep_dict)\
.subs(steady_conditions).subs({q[3]: -r*cos(q[1])}).expand()
# Second, using KaneMethod in mechanics for fr, frstar and frstar_steady.
# Rigid Bodies: disc, with inertia I_C_O.
iner_tuple = (I_C_O, O)
disc = RigidBody('disc', O, C, m, iner_tuple)
bodyList = [disc]
# Generalized forces: Gravity: F_o; Auxiliary forces: F_p.
F_o = (O, F_O)
F_p = (P, F_P)
forceList = [F_o, F_p]
# KanesMethod.
kane = KanesMethod(
N, q_ind= q[:3], u_ind= u[:3], kd_eqs=kindiffs,
q_dependent=q[3:], configuration_constraints = f_c,
u_dependent=u[3:], velocity_constraints= f_v,
u_auxiliary=ua
)
# fr, frstar, frstar_steady and kdd(kinematic differential equations).
(fr, frstar)= kane.kanes_equations(forceList, bodyList)
frstar_steady = frstar.subs(ud_zero).subs(u_dep_dict).subs(steady_conditions)\
.subs({q[3]: -r*cos(q[1])}).expand()
kdd = kane.kindiffdict()
# Test
# First try Fr_c == fr;
# Second try Fr_star_c == frstar;
# Third try Fr_star_steady == frstar_steady.
# Both signs are checked in case the equations were found with an inverse
# sign.
assert ((Matrix(Fr_c).expand() == fr.expand()) or
(Matrix(Fr_c).expand() == (-fr).expand()))
assert ((Matrix(Fr_star_c).expand() == frstar.expand()) or
(Matrix(Fr_star_c).expand() == (-frstar).expand()))
assert ((Matrix(Fr_star_steady).expand() == frstar_steady.expand()) or
(Matrix(Fr_star_steady).expand() == (-frstar_steady).expand()))
def test_linearize_rolling_disc_kane():
# Symbols for time and constant parameters
t, r, m, g, v = symbols('t r m g v')
# Configuration variables and their time derivatives
q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7')
q1d, q2d, q3d, q4d, q5d, q6d = qd = [qi.diff(t) for qi in q]
# Generalized speeds and their time derivatives
u = dynamicsymbols('u:6')
u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7')
u1d, u2d, u3d, u4d, u5d, u6d = [ui.diff(t) for ui in u]
# Reference frames
N = ReferenceFrame('N') # Inertial frame
NO = Point('NO') # Inertial origin
A = N.orientnew('A', 'Axis', [q1, N.z]) # Yaw intermediate frame
B = A.orientnew('B', 'Axis', [q2, A.x]) # Lean intermediate frame
C = B.orientnew('C', 'Axis', [q3, B.y]) # Disc fixed frame
CO = NO.locatenew('CO', q4*N.x + q5*N.y + q6*N.z) # Disc center
# Disc angular velocity in N expressed using time derivatives of coordinates
w_c_n_qd = C.ang_vel_in(N)
w_b_n_qd = B.ang_vel_in(N)
# Inertial angular velocity and angular acceleration of disc fixed frame
C.set_ang_vel(N, u1*B.x + u2*B.y + u3*B.z)
# Disc center velocity in N expressed using time derivatives of coordinates
v_co_n_qd = CO.pos_from(NO).dt(N)
# Disc center velocity in N expressed using generalized speeds
CO.set_vel(N, u4*C.x + u5*C.y + u6*C.z)
# Disc Ground Contact Point
P = CO.locatenew('P', r*B.z)
P.v2pt_theory(CO, N, C)
# Configuration constraint
f_c = Matrix([q6 - dot(CO.pos_from(P), N.z)])
# Velocity level constraints
f_v = Matrix([dot(P.vel(N), uv) for uv in C])
# Kinematic differential equations
kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
[dot(v_co_n_qd - CO.vel(N), uv) for uv in N])
qdots = solve(kindiffs, qd)
# Set angular velocity of remaining frames
B.set_ang_vel(N, w_b_n_qd.subs(qdots))
C.set_ang_acc(N, C.ang_vel_in(N).dt(B) + cross(B.ang_vel_in(N), C.ang_vel_in(N)))
# Active forces
F_CO = m*g*A.z
# Create inertia dyadic of disc C about point CO
I = (m * r**2) / 4
J = (m * r**2) / 2
I_C_CO = inertia(C, I, J, I)
Disc = RigidBody('Disc', CO, C, m, (I_C_CO, CO))
BL = [Disc]
FL = [(CO, F_CO)]
KM = KanesMethod(N, [q1, q2, q3, q4, q5], [u1, u2, u3], kd_eqs=kindiffs,
q_dependent=[q6], configuration_constraints=f_c,
u_dependent=[u4, u5, u6], velocity_constraints=f_v)
(fr, fr_star) = KM.kanes_equations(FL, BL)
# Test generalized form equations
linearizer = KM.to_linearizer()
assert linearizer.f_c == f_c
assert linearizer.f_v == f_v
assert linearizer.f_a == f_v.diff(t)
sol = solve(linearizer.f_0 + linearizer.f_1, qd)
for qi in qd:
assert sol[qi] == qdots[qi]
assert simplify(linearizer.f_2 + linearizer.f_3 - fr - fr_star) == Matrix([0, 0, 0])
# Perform the linearization
# Precomputed operating point
q_op = {q6: -r*cos(q2)}
u_op = {u1: 0,
u2: sin(q2)*q1d + q3d,
u3: cos(q2)*q1d,
u4: -r*(sin(q2)*q1d + q3d)*cos(q3),
u5: 0,
u6: -r*(sin(q2)*q1d + q3d)*sin(q3)}
qd_op = {q2d: 0,
q4d: -r*(sin(q2)*q1d + q3d)*cos(q1),
q5d: -r*(sin(q2)*q1d + q3d)*sin(q1),
q6d: 0}
ud_op = {u1d: 4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5,
u2d: 0,
u3d: 0,
u4d: r*(sin(q2)*sin(q3)*q1d*q3d + sin(q3)*q3d**2),
u5d: r*(4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5),
u6d: -r*(sin(q2)*cos(q3)*q1d*q3d + cos(q3)*q3d**2)}
A, B = linearizer.linearize(op_point=[q_op, u_op, qd_op, ud_op], A_and_B=True, simplify=True)
upright_nominal = {q1d: 0, q2: 0, m: 1, r: 1, g: 1}
# Precomputed solution
A_sol = Matrix([[0, 0, 0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0],
[sin(q1)*q3d, 0, 0, 0, 0, -sin(q1), -cos(q1), 0],
[-cos(q1)*q3d, 0, 0, 0, 0, cos(q1), -sin(q1), 0],
[0, 4/5, 0, 0, 0, 0, 0, 6*q3d/5],
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, -2*q3d, 0, 0]])
B_sol = Matrix([])
# Check that linearization is correct
assert A.subs(upright_nominal) == A_sol
assert B.subs(upright_nominal) == B_sol
# Check eigenvalues at critical speed are all zero:
assert A.subs(upright_nominal).subs(q3d, 1/sqrt(3)).eigenvals() == {0: 8}
def test_rolling_disc():
# Rolling Disc Example
# Here the rolling disc is formed from the contact point up, removing the
# need to introduce generalized speeds. Only 3 configuration and three
# speed variables are need to describe this system, along with the disc's
# mass and radius, and the local gravity (note that mass will drop out).
q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
r, m, g = symbols('r m g')
# The kinematics are formed by a series of simple rotations. Each simple
# rotation creates a new frame, and the next rotation is defined by the new
# frame's basis vectors. This example uses a 3-1-2 series of rotations, or
# Z, X, Y series of rotations. Angular velocity for this is defined using
# the second frame's basis (the lean frame).
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Axis', [q1, N.z])
L = Y.orientnew('L', 'Axis', [q2, Y.x])
R = L.orientnew('R', 'Axis', [q3, L.y])
w_R_N_qd = R.ang_vel_in(N)
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
# This is the translational kinematics. We create a point with no velocity
# in N; this is the contact point between the disc and ground. Next we form
# the position vector from the contact point to the disc's center of mass.
# Finally we form the velocity and acceleration of the disc.
C = Point('C')
C.set_vel(N, 0)
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
# This is a simple way to form the inertia dyadic.
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
# Kinematic differential equations; how the generalized coordinate time
# derivatives relate to generalized speeds.
kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]
# Creation of the force list; it is the gravitational force at the mass
# center of the disc. Then we create the disc by assigning a Point to the
# center of mass attribute, a ReferenceFrame to the frame attribute, and mass
# and inertia. Then we form the body list.
ForceList = [(Dmc, - m * g * Y.z)]
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
BodyList = [BodyD]
# Finally we form the equations of motion, using the same steps we did
# before. Specify inertial frame, supply generalized speeds, supply
# kinematic differential equation dictionary, compute Fr from the force
# list and Fr* from the body list, compute the mass matrix and forcing
# terms, then solve for the u dots (time derivatives of the generalized
# speeds).
KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd)
KM.kanes_equations(ForceList, BodyList)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
kdd = KM.kindiffdict()
rhs = rhs.subs(kdd)
rhs.simplify()
assert rhs.expand() == Matrix([(6*u2*u3*r - u3**2*r*tan(q2) +
4*g*sin(q2))/(5*r), -2*u1*u3/3, u1*(-2*u2 + u3*tan(q2))]).expand()
# This code tests our output vs. benchmark values. When r=g=m=1, the
# critical speed (where all eigenvalues of the linearized equations are 0)
# is 1 / sqrt(3) for the upright case.
A = KM.linearize(A_and_B=True, new_method=True)[0]
A_upright = A.subs({r: 1, g: 1, m: 1}).subs({q1: 0, q2: 0, q3: 0, u1: 0, u3: 0})
assert A_upright.subs(u2, 1 / sqrt(3)).eigenvals() == {S(0): 6}
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""Exercise 7.2 from Kane 1985."""
from __future__ import division
from sympy import solve, symbols
from sympy.physics.mechanics import ReferenceFrame, Point
from sympy.physics.mechanics import cross, dot
c1, c2, c3 = symbols('c1 c2 c3', real=True)
alpha = 10 # [N]
N = ReferenceFrame('N')
pO = Point('O')
pS = pO.locatenew('S', 10*N.x + 5*N.z) # [m]
pR = pO.locatenew('R', 10*N.x + 12*N.y) # [m]
pQ = pO.locatenew('Q', 12*N.y + 10*N.z) # [m]
pP = pO.locatenew('P', 4*N.x + 7*N.z) # [m]
A = alpha * pQ.pos_from(pP).normalize()
B = alpha * pS.pos_from(pR).normalize()
C_ = c1*N.x + c2*N.y + c3*N.z
eqs = [dot(A + B + C_, b) for b in N]
soln = solve(eqs, [c1, c2, c3])
C = sum(soln[ci] * b
for ci, b in zip(sorted(soln, cmp=lambda x, y: x.compare(y)), N))
print("C = {0}\nA + B + C = {1}".format(C, A + B + C))
M = cross(pP.pos_from(pO), A) + cross(pR.pos_from(pO), B)
print("|M| = {0} N-m".format(M.magnitude().n()))
# C* is the point at the center of disk C.
pCs = pC_hat.locatenew('C*', R*B.y)
pCs.set_vel(C, 0)
pCs.set_vel(B, 0)
# calculate velocities in A
pCs.v2pt_theory(pR, A, B)
pC_hat.v2pt_theory(pCs, A, C)
print("velocities of points R, C^, C* in rf A:")
print("v_R_A = {0}\nv_C^_A = {1}\nv_C*_A = {2}".format(
pR.vel(A), pC_hat.vel(A), pCs.vel(A)))
## --- Expressions for generalized speeds u1, u2, u3, u4, u5 ---
u_expr = map(lambda x: dot(C.ang_vel_in(A), x), B)
u_expr += qd[3:]
kde = [u_i - u_ex for u_i, u_ex in zip(u, u_expr)]
kde_map = solve(kde, qd)
print("using the following kinematic eqs:\n{0}".format(msprint(kde)))
## --- Define forces on each point in the system ---
R_C_hat = Px*A.x + Py*A.y + Pz*A.z
R_Cs = -m*g*A.z
forces = [(pC_hat, R_C_hat), (pCs, R_Cs)]
## --- Calculate generalized active forces ---
partials = partial_velocities([pC_hat, pCs], u, A, kde_map)
F, _ = generalized_active_forces(partials, forces)
print("Generalized active forces:")
for i, f in enumerate(F, 1):
def test_non_central_inertia():
# This tests that the calculation of Fr* does not depend the point
# about which the inertia of a rigid body is defined. This test solves
# exercises 8.12, 8.17 from Kane 1985.
# Declare symbols
q1, q2, q3 = dynamicsymbols('q1:4')
q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
u1, u2, u3, u4, u5 = dynamicsymbols('u1:6')
u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta')
a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
Q1, Q2, Q3 = symbols('Q1, Q2 Q3')
IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
# Reference Frames
F = ReferenceFrame('F')
P = F.orientnew('P', 'axis', [-theta, F.y])
A = P.orientnew('A', 'axis', [q1, P.x])
A.set_ang_vel(F, u1*A.x + u3*A.z)
# define frames for wheels
B = A.orientnew('B', 'axis', [q2, A.z])
C = A.orientnew('C', 'axis', [q3, A.z])
B.set_ang_vel(A, u4 * A.z)
C.set_ang_vel(A, u5 * A.z)
# define points D, S*, Q on frame A and their velocities
pD = Point('D')
pD.set_vel(A, 0)
# u3 will not change v_D_F since wheels are still assumed to roll without slip.
pD.set_vel(F, u2 * A.y)
pS_star = pD.locatenew('S*', e*A.y)
pQ = pD.locatenew('Q', f*A.y - R*A.x)
for p in [pS_star, pQ]:
p.v2pt_theory(pD, F, A)
# masscenters of bodies A, B, C
pA_star = pD.locatenew('A*', a*A.y)
pB_star = pD.locatenew('B*', b*A.z)
pC_star = pD.locatenew('C*', -b*A.z)
for p in [pA_star, pB_star, pC_star]:
p.v2pt_theory(pD, F, A)
# points of B, C touching the plane P
pB_hat = pB_star.locatenew('B^', -R*A.x)
pC_hat = pC_star.locatenew('C^', -R*A.x)
pB_hat.v2pt_theory(pB_star, F, B)
pC_hat.v2pt_theory(pC_star, F, C)
# the velocities of B^, C^ are zero since B, C are assumed to roll without slip
kde = [q1d - u1, q2d - u4, q3d - u5]
vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]
# inertias of bodies A, B, C
# IA22, IA23, IA33 are not specified in the problem statement, but are
# necessary to define an inertia object. Although the values of
# IA22, IA23, IA33 are not known in terms of the variables given in the
# problem statement, they do not appear in the general inertia terms.
inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
inertia_B = inertia(B, K, K, J)
inertia_C = inertia(C, K, K, J)
# define the rigid bodies A, B, C
rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
km = KanesMethod(F, q_ind=[q1, q2, q3], u_ind=[u1, u2], kd_eqs=kde,
u_dependent=[u4, u5], velocity_constraints=vc,
u_auxiliary=[u3])
forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x + Q2*A.y + Q3*A.z)]
bodies = [rbA, rbB, rbC]
fr, fr_star = km.kanes_equations(forces, bodies)
vc_map = solve(vc, [u4, u5])
# KanesMethod returns the negative of Fr, Fr* as defined in Kane1985.
fr_star_expected = Matrix([
-(IA + 2*J*b**2/R**2 + 2*K +
mA*a**2 + 2*mB*b**2) * u1.diff(t) - mA*a*u1*u2,
-(mA + 2*mB +2*J/R**2) * u2.diff(t) + mA*a*u1**2,
0])
assert (trigsimp(fr_star.subs(vc_map).subs(u3, 0)).doit().expand() ==
fr_star_expected.expand())
# define inertias of rigid bodies A, B, C about point D
# I_S/O = I_S/S* + I_S*/O
bodies2 = []
for rb, I_star in zip([rbA, rbB, rbC], [inertia_A, inertia_B, inertia_C]):
I = I_star + inertia_of_point_mass(rb.mass,
rb.masscenter.pos_from(pD),
rb.frame)
bodies2.append(RigidBody('', rb.masscenter, rb.frame, rb.mass,
(I, pD)))
fr2, fr_star2 = km.kanes_equations(forces, bodies2)
assert (trigsimp(fr_star2.subs(vc_map).subs(u3, 0)).doit().expand() ==
fr_star_expected.expand())
x, y = _me.dynamicsymbols('x y')
x_d, y_d = _me.dynamicsymbols('x_ y_', 1)
e = _sm.cos(x)+_sm.sin(x)+_sm.tan(x)+_sm.cosh(x)+_sm.sinh(x)+_sm.tanh(x)+_sm.acos(x)+_sm.asin(x)+_sm.atan(x)+_sm.log(x)+_sm.exp(x)+_sm.sqrt(x)+_sm.factorial(x)+_sm.ceiling(x)+_sm.floor(x)+_sm.sign(x)
e = (x)**2+_sm.log(x, 10)
a = _sm.Abs(-1*1)+int(1.5)+round(1.9)
e1 = 2*x+3*y
e2 = x+y
am = _sm.Matrix([e1.expand().coeff(x), e1.expand().coeff(y), e2.expand().coeff(x), e2.expand().coeff(y)]).reshape(2, 2)
b = (e1).expand().coeff(x)
c = (e2).expand().coeff(y)
d1 = (e1).collect(x).coeff(x,0)
d2 = (e1).collect(x).coeff(x,1)
fm = _sm.Matrix([i.collect(x)for i in _sm.Matrix([e1,e2]).reshape(1, 2)]).reshape((_sm.Matrix([e1,e2]).reshape(1, 2)).shape[0], (_sm.Matrix([e1,e2]).reshape(1, 2)).shape[1])
f = (e1).collect(y)
g = (e1).subs({x:2*x})
gm = _sm.Matrix([i.subs({x:3}) for i in _sm.Matrix([e1,e2]).reshape(2, 1)]).reshape((_sm.Matrix([e1,e2]).reshape(2, 1)).shape[0], (_sm.Matrix([e1,e2]).reshape(2, 1)).shape[1])
frame_a = _me.ReferenceFrame('a')
frame_b = _me.ReferenceFrame('b')
theta = _me.dynamicsymbols('theta')
frame_b.orient(frame_a, 'Axis', [theta, frame_a.z])
v1 = 2*frame_a.x-3*frame_a.y+frame_a.z
v2 = frame_b.x+frame_b.y+frame_b.z
a = _me.dot(v1, v2)
bm = _sm.Matrix([_me.dot(v1, v2),_me.dot(v1, 2*v2)]).reshape(2, 1)
c = _me.cross(v1, v2)
d = 2*v1.magnitude()+3*v1.magnitude()
dyadic = _me.outer(3*frame_a.x, frame_a.x)+_me.outer(frame_a.y, frame_a.y)+_me.outer(2*frame_a.z, frame_a.z)
am = (dyadic).to_matrix(frame_b)
m = _sm.Matrix([1,2,3]).reshape(3, 1)
v = m[0]*frame_a.x +m[1]*frame_a.y +m[2]*frame_a.z
def calc_torque(panel, force, bus):
force_z = mech.dot(force, bus.y) * sp.cos(get_angle(panel.frame, bus))
#force_z = mech.dot(force, panel.frame.y).evalf()
torque_zz = -panel.dimensions[1] / 2 * force_z
panel.torque = 0 * panel.frame.y + 0 * panel.frame.x + torque_zz * panel.frame.z
return force_z
B.set_ang_vel(A, u2 * A["1"])
C.set_ang_vel(B, u3 * B["2"])
E.set_ang_vel(C, u4 * C["3"])
D.set_ang_vel(C, u5 * C["2"])
F.set_ang_vel(E, u6 * E["2"])
print("ready for special unit vectors; and points and velocities")
######################
# special unit vectors
######################
# pitch back for the unit vector g_3 along front wheel radius;
g_3 = (mec.express(A["3"], E) - mec.dot(E["2"], A["3"]) * E["2"]).normalize()
# another way: g_3 = E['2'].cross(A['3']).cross(E['2']).normalize()
# roll back for longitudinal and lateral unit vector of front wheel
long_v = mec.cross(E["2"], A["3"]).normalize()
lateral_v = mec.cross(A["3"], long_v).normalize()
#########################
# points and velocities
#########################
####################rear wheel contact point, dn
dn = mec.Point("dn")
# points and velocities
pO = Point('O')
pO.set_vel(A, 0)
pO.set_vel(B, 0)
pP1 = pO.locatenew('P1', q1 * B.x + q2 * B.y)
pDs = pP1.locatenew('D*', L * E.x)
pP1.set_vel(E, 0)
pP1.set_vel(B, pP1.pos_from(pO).dt(B))
pP1.v1pt_theory(pO, A, B)
pDs.set_vel(E, 0)
pDs.v2pt_theory(pP1, B, E)
pDs.v2pt_theory(pP1, A, E)
# define generalized speeds and constraints
kde = [u1 - dot(pP1.vel(A), E.x), u2 - dot(pP1.vel(A), E.y), u3 - q3d]
kde_map = solve(kde, qd)
# include second derivatives in kde map
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
vc = [dot(pDs.vel(B), E.y)]
vc_map = solve(subs(vc, kde_map), [u3])
# define system of particles
system = [Particle('P1', pP1, m1), Particle('P2', pDs, m2)]
# calculate kinetic energy, generalized inertia forces
K = sum(map(lambda x: x.kinetic_energy(A), system))
Fr_tilde_star = generalized_inertia_forces_K(K, q, [u1, u2], kde_map, vc_map)
