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, manual 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 = dict(zip(ud, [0.] * len(ud)))
ua = dynamicsymbols('ua:3')
ua_zero = dict(zip(ua, [0.] * len(ua))) # noqa:F841
# 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) # noqa:F841
# 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]) # noqa:F841
# 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))
# 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).
with warns_deprecated_sympy():
(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()
assert Matrix(Fr_c).expand() == fr.expand()
assert Matrix(Fr_star_c.subs(kdd)).expand() == frstar.expand()
assert (simplify(Matrix(Fr_star_steady).expand()) == simplify(
frstar_steady.expand()))
syms_in_forcing = find_dynamicsymbols(kane.forcing)
for qdi in qd:
assert qdi not in syms_in_forcing
w: 1,
f: 2,
v0: 20}
N = ReferenceFrame('N')
B = N.orientnew('B', 'axis', [q3, N.z])
O = Point('O')
S = O.locatenew('S', q1*N.x + q2*N.y)
S.set_vel(N, S.pos_from(O).dt(N))
#Is = m/12*(l**2 + w**2)
Is = symbols('Is')
I = inertia(B, 0, 0, Is, 0, 0, 0)
rb = RigidBody('rb', S, B, m, (I, S))
rb.set_potential_energy(0)
L = Lagrangian(N, rb)
lm = LagrangesMethod(
L, q, nonhol_coneqs = [q1d*sin(q3) - q2d*cos(q3) + l/2*q3d])
lm.form_lagranges_equations()
rhs = lm.rhs()
print('{} = {}'.format(msprint(q1d.diff(t)),
msprint(rhs[3].simplify())))
print('{} = {}'.format(msprint(q2d.diff(t)),
msprint(rhs[4].simplify())))
print('{} = {}'.format(msprint(q3d.diff(t)),
msprint(rhs[5].simplify())))
print('{} = {}'.format('λ', msprint(rhs[6].simplify())))
from sympy.physics.mechanics import inertia, msprint
from sympy.physics.mechanics import Point, RigidBody
from sympy import pi, solve, symbols, simplify
from sympy import acos, sin, cos
# 2a
q1 = dynamicsymbols('q1')
px, py, pz = symbols('px py pz', real=True, positive=True)
sx, sy, sz = symbols('sx sy sz', real=True, sositive=True)
m, g, l0, k = symbols('m g l0 k', real=True, positive=True)
Ixx, Iyy, Izz = symbols('Ixx Iyy Izz', real=True, positive=True)
N = ReferenceFrame('N')
B = N.orientnew('B', 'axis', [q1, N.z])
pA = Point('A')
pA.set_vel(N, 0)
pP = pA.locatenew('P', l0 * N.y - 2 * l0 * N.z)
pS = pP.locatenew('S', -px * B.x - pz * B.z)
I = inertia(B, Ixx, Iyy, Izz, 0, 0, 0)
rb = RigidBody('plane', pS, B, m, (I, pS))
H = rb.angular_momentum(pS, N)
print('H about S in frame N = {}'.format(msprint(H)))
print('dH/dt = {}'.format(msprint(H.dt(N))))
print('a_S_N = {}'.format(pS.acc(N)))
# 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)))
u_indep = [u1, u2, u3]
Fr = generalized_active_forces_K(K, q, u_indep, kde_map, vc_map)
# Fr + Fr* = 0 but the dynamical equations cannot be formulated by only
# kinetic energy as Fr = -Fr* for r = 1, ..., p
print('\ngeneralized active forces, Fr')
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)
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 graivty (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])
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
R.set_ang_acc(N,
R.ang_vel_in(N).dt(R) + (R.ang_vel_in(N) ^ R.ang_vel_in(N)))
# 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 mass center.
# 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)
Dmc.a2pt_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 = [q1d - u3 / cos(q3), q2d - u1, q3d - u2 + u3 * tan(q2)]
# 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
# mass center 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.mc = Dmc
BodyD.inertia = (I, Dmc)
BodyD.frame = R
BodyD.mass = m
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* fromt the body list, compute the mass matrix and forcing
# terms, then solve for the u dots (time derivatives of the generalized
# speeds).
KM = Kane(N)
KM.coords([q1, q2, q3])
KM.speeds([u1, u2, u3])
KM.kindiffeq(kd)
KM.kanes_equations(ForceList, BodyList)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
kdd = KM.kindiffdict()
rhs = rhs.subs(kdd)
assert rhs.expand() == Matrix([
(10 * u2 * u3 * r - 5 * u3**2 * r * tan(q2) + 4 * g * sin(q2)) /
(5 * r), -2 * u1 * u3 / 3, u1 * (-2 * u2 + u3 * tan(q2))
]).expand()
q1, q2, q3 = dynamicsymbols('q1, q2 q3')
#omega1, omega2, omega3 = dynamicsymbols('ω1 ω2 ω3')
q1d, q2d = dynamicsymbols('q1, q2', level=1)
m, I11, I22, I33 = symbols('m I11 I22 I33', real=True, positive=True)
# 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)))
R = J1.locatenew('R', 0.5 * ob * B.y)
T = O.locatenew('T', 0.5 * ta * C.y)
P.v2pt_theory(J0, N, A)
R.v2pt_theory(J1, N, B)
T.v2pt_theory(O, N, C)
# Bodies
IP = inertia(A, 1 / 12 * m * (3 * r**2 + bb**2), 0,
1 / 12 * m * (3 * r**2 + bb**2))
IP_tuple = (IP, P)
IR = inertia(B, 1 / 12 * m * (3 * r**2 + ob**2), 0,
1 / 12 * m * (3 * r**2 + ob**2))
IR_tuple = (IR, R)
IBody = inertia(C, 1 / 12 * 0.1 * m * (ta)**2, 0, 1 / 12 * 0.1 * m * (ta)**2)
IBody_tuple = (IBody, T)
BodyP = RigidBody('BodyP', P, A, m, IP_tuple)
BodyR = RigidBody('BodyR', R, B, m, IR_tuple)
Pedaal = RigidBody('Pedaal', T, C, 0.1 * m, IBody_tuple)
#Force list and body list
FL = [(P, m * g * N.y), (R, m * g * N.y), (A, (-75 * q1 - 0.01 * u1) * A.z),
(B, (-25 * q2 * B.z)), (C, (-25 * C.z))]
BL = [BodyP, BodyR, Pedaal]
# Define configuration constraints and obtain velocity constraints
cc = [J2.pos_from(S).magnitude()]
kd = [q1.diff() - u1, q1.diff() + q2.diff() - u2, theta.diff() - omega]
kd_map = solve(kd, [qi.diff() for qi in [q1, q2, theta]])
vc = [x.subs(kd_map) for x in [cc[0].diff(dynamicsymbols._t)]]
constants = [m, g, r, ob, bb, ta, d1, d2]
# Point C* is moving in Reference Frame B
pC_star.set_vel(B, pC_star.pos_from(pB_star).diff(t, B))
pC_star.v1pt_theory(pB_star, E, B)
pD_star.set_vel(B, pC_star.vel(B)) # Point D* is fixed rel to Point C* in B
pD_star.v1pt_theory(pB_star, E, B) # Point D* is moving in Reference Frame B
# --- define central inertias and rigid bodies ---
IA = inertia(A, A1, A2, A3)
IB = inertia(B, B1, B2, B3)
IC = inertia(B, C1, C2, C3)
ID = inertia(B, D11, D22, D33, D12, D23, D31)
# inertia[0] is defined to be the central inertia for each rigid body
rbA = RigidBody('rbA', pA_star, A, mA, (IA, pA_star))
rbB = RigidBody('rbB', pB_star, B, mB, (IB, pB_star))
rbC = RigidBody('rbC', pC_star, C, mC, (IC, pC_star))
rbD = RigidBody('rbD', pD_star, D, mD, (ID, pD_star))
bodies = [rbA, rbB, rbC, rbD]
## --- generalized speeds ---
kde = [
u1 - dot(A.ang_vel_in(E), A.x), u2 - dot(B.ang_vel_in(A), B.y),
u3 - dot(pC_star.vel(B), B.z)
]
kde_map = solve(kde, [q0d, q1d, q2d])
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
print('\nEx8.20')
pC_star = Point('pC*') # center of mass of rod
pA_star = pC_star.locatenew('pA*', L / 2 * C.x) # center of disk A
pB_star = pC_star.locatenew('pB*', -L / 2 * C.x) # center of disk A
pA_hat = pA_star.locatenew('pA^',
-r * C.z) # contact point of disk A and ground
pB_hat = pB_star.locatenew('pB^',
-r * C.z) # contact point of disk A and ground
pC_star.set_vel(N, v * C.y)
pA_star.v2pt_theory(pC_star, N, C) # pA* and pC* are both fixed in frame C
pB_star.v2pt_theory(pC_star, N, C) # pB* and pC* are both fixed in frame C
pA_hat.v2pt_theory(pA_star, N, A) # pA* and pA^ are both fixed in frame A
pB_hat.v2pt_theory(pB_star, N, B) # pB* and pB^ are both fixed in frame B
I_rod = inertia(C, 0, m0 * L**2 / 12, m0 * L**2 / 12, 0, 0, 0)
rbC = RigidBody('rod_C', pC_star, C, m0, (I_rod, pC_star))
I_discA = inertia(A, m * r**2 / 2, m * r**2 / 4, m * r**2 / 4, 0, 0, 0)
rbA = RigidBody('disc_A', pA_star, A, m, (I_discA, pA_star))
I_discB = inertia(B, m * r**2 / 2, m * r**2 / 4, m * r**2 / 4, 0, 0, 0)
rbB = RigidBody('disc_B', pB_star, B, m, (I_discB, pB_star))
print('omega_A_N = {}'.format(msprint(A.ang_vel_in(N).express(C))))
print('v_pA*_N = {}'.format(msprint(pA_hat.vel(N))))
qd_val = solve([dot(pA_hat.vel(N), C.y), dot(pB_hat.vel(N), C.y)], [q2d, q3d])
print(msprint(qd_val))
print('T_A = {}'.format(msprint(simplify(rbA.kinetic_energy(N).subs(qd_val)))))
print('T_B = {}'.format(msprint(simplify(rbB.kinetic_energy(N).subs(qd_val)))))
def __init__(self):
self.coords = dynamicsymbols('q:3')
self.speeds = dynamicsymbols('p:3')
self.dynamic = list(self.coords) + list(self.speeds)
self.states = ([radians(45) for x in self.coords] +
[radians(30) for x in self.speeds])
self.inertial_ref_frame = ReferenceFrame('I')
self.A = self.inertial_ref_frame.orientnew('A', 'space',
self.coords, 'XYZ')
self.B = self.A.orientnew('B', 'space', self.speeds, 'XYZ')
self.camera_ref_frame = \
self.inertial_ref_frame.orientnew('C', 'space', (pi / 2, 0, 0),
'XYZ')
self.origin = Point('O')
self.P1 = self.origin.locatenew('P1',
10 * self.inertial_ref_frame.x +
10 * self.inertial_ref_frame.y +
10 * self.inertial_ref_frame.z)
self.P2 = self.P1.locatenew('P2',
10 * self.inertial_ref_frame.x +
10 * self.inertial_ref_frame.y +
10 * self.inertial_ref_frame.z)
self.point_list1 = [[2, 3, 1], [4, 6, 2], [5, 3, 1], [5, 3, 6]]
self.point_list2 = [[3, 1, 4], [3, 8, 2], [2, 1, 6], [2, 1, 1]]
self.shape1 = Cylinder(1.0, 1.0)
self.shape2 = Cylinder(1.0, 1.0, name='booger')
self.Ixx, self.Iyy, self.Izz = symbols('Ixx, Iyy, Izz')
self.mass = symbols('m')
self.parameters = [self.Ixx, self.Iyy, self.Izz, self.mass]
self.param_vals = [2.0, 3.0, 4.0, 5.0]
self.inertia = inertia(self.A, self.Ixx, self.Iyy, self.Izz)
self.rigid_body = RigidBody('rigid_body1', self.P1, self.A,
self.mass, (self.inertia, self.P1))
self.global_frame1 = VisualizationFrame('global_frame1', self.A,
self.P1, self.shape1)
self.global_frame2 = VisualizationFrame('global_frame2', self.B,
self.P2, self.shape2)
self.scene1 = Scene(self.inertial_ref_frame, self.origin,
(self.global_frame1, self.global_frame2),
name='scene')
self.particle = Particle('particle1', self.P1, self.mass)
q = self.coords
c = cos
s = sin
# Here is the dragon ..
self.transformation_matrix = [
[c(q[1])*c(q[2]), s(q[2])*c(q[1]), -s(q[1]), 0],
[s(q[0])*s(q[1])*c(q[2]) - s(q[2])*c(q[0]),
s(q[0])*s(q[1])*s(q[2]) + c(q[0])*c(q[2]),
s(q[0])*c(q[1]), 0],
[s(q[0])*s(q[2]) + s(q[1])*c(q[0])*c(q[2]),
-s(q[0])*c(q[2]) + s(q[1])*s(q[2])*c(q[0]),
c(q[0])*c(q[1]), 0],
[10, 10, 10, 1]]
O = Point('O')
O.set_vel(B0, 0)
A = Point('A')
A.set_pos(O, L_1 * B1.x)
A.v2pt_theory(O, B0, B1)
CM_1 = Point('CM_1')
CM_1.set_pos(O, R_1 * B1.x)
CM_1.v2pt_theory(O, B0, B1)
CM_2 = Point('CM_2')
CM_2.set_pos(A, R_2 * B2.x)
CM_2.v2pt_theory(O, B0, B2)
# Corpos Rígidos
I_1 = inertia(B1, 0, 0, I_1_ZZ)
# Elo 1
E_1 = RigidBody('Elo_1', CM_1, B1, M_1, (I_1, CM_1))
I_2 = inertia(B1, 0, 0, I_1_ZZ)
# Elo 2
E_2 = RigidBody('Elo_2', CM_2, B2, M_2, (I_2, CM_2))
# Energia Potencial
P_1 = -M_1 * G * B0.z
R_1_CM = CM_1.pos_from(O).express(B0)
E_1.potential_energy = R_1_CM.dot(P_1)
P_2 = -M_2 * G * B0.z
R_2_CM = CM_2.pos_from(O).express(B0).simplify()
E_2.potential_energy = R_2_CM.dot(P_2)
# Forças/Momentos Generalizados
FL = [(B1, TAU_1 * B0.z), (B2, TAU_2 * B0.z)]
# Método de Lagrange
com2.v2pt_theory(com_cart, N, link2_frame)
com3 = A.locatenew("com3", 0.5*link3_frame.y*d_links[1]) # Link 3
com3.v2pt_theory(A, N, link3_frame)
com4 = B.locatenew("com4", 0.5*link4_frame.y*d_links[1]) # Link 4
com4.v2pt_theory(B, N, link4_frame)
com = (com1, com2, com3, com4)
# ------------------------------- Rigid bodies -------------------------------
for i in range(4):
j = floor(i/2) # Index for link mass and length
links.append(RigidBody(f"link{i}", com[i], frames[i],
m_links[j],
(inertia(frames[0], 0, 0, m_links[j]/12
* d_links[j]**2), com[i])))
# ----------------------------------------------------------------------------
# Compute energy expressions
# ----------------------------------------------------------------------------
def set_pot_grav_energy(thing):
# Set the potential gravitational energy for either a Particle or RigidBody
# based on its height in the inertial frame N.
try:
point = thing.point
except AttributeError:
point = thing.masscenter
inertia_C = inertia(C, 0.010932, 0.011127, 0.001043, 0.000000, 0.000606,
-0.000007)
inertia_D = inertia(D, 0.008147, 0.000631, 0.008316, -0.000001, -0.000500,
0.000000)
inertia_E = inertia(E, 0.001596, 0.001607, 0.000399, 0.000000, 0.000256,
0.000000)
inertia_F = inertia(F, 0.001641, 0.000410, 0.001641, 0.000000, -0.000278,
0.000000)
inertia_G = inertia(G, 0.000587, 0.000369, 0.000609, 0.000003, 0.000118,
0.000003)
# Inertia of the gripper
if gripper:
inertia_H = inertia(H, 4180e-6, 5080e-6, 1250e-6)
body_A = RigidBody('body_A', Ao, A, mass_A, (inertia_A, Ao))
body_B = RigidBody('body_B', Bo, B, mass_B, (inertia_B, Bo))
body_C = RigidBody('body_C', Co, C, mass_C, (inertia_C, Co))
body_D = RigidBody('body_D', Do, D, mass_D, (inertia_D, Do))
body_E = RigidBody('body_E', Eo, E, mass_E, (inertia_E, Eo))
body_F = RigidBody('body_F', Fo, F, mass_F, (inertia_F, Fo))
body_G = RigidBody('body_G', Go, G, mass_G, (inertia_G, Go))
if gripper:
body_H = RigidBody('body_H', Ho, H, mass_H, (inertia_H, Ho))
body_list = [body_A, body_B, body_C, body_D, body_E, body_F, body_G, body_H] \
if gripper else [body_A, body_B, body_C, body_D, body_E, body_F, body_G]
force_list = [(Ao, N.x * g * mass_A),
(Bo, N.x * g * mass_B),
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]
with warns_deprecated_sympy():
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
])
t = trigsimp(fr_star.subs(vc_map).subs({u3: 0})).doit().expand()
assert ((fr_star_expected - t).expand() == zeros(3, 1))
# 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)))
with warns_deprecated_sympy():
fr2, fr_star2 = km.kanes_equations(forces, bodies2)
t = trigsimp(fr_star2.subs(vc_map).subs({u3: 0})).doit()
assert (fr_star_expected - t).expand() == zeros(3, 1)
q1, q2, q3 = q = dynamicsymbols('q1:4')
q1d, q2d, q3d = qd = dynamicsymbols('q1:4', level=1)
u1, u2, u3 = u = dynamicsymbols('u1:4')
m, IB11, IB22, IB33 = symbols('m IB11 IB22 IB33')
# reference frames
A = ReferenceFrame('A')
B = A.orientnew('B', 'body', [q1, q2, q3], 'xyz')
# define a point fixed in both A and B
pO = Point('O')
pO.set_vel(A, 0)
# define the rigid body B
I = inertia(B, IB11, IB22, IB33)
rb = RigidBody('rbB', pO, B, m, (I, pO))
def coupled_speeds(ic_matrix, partials):
ulist = partials.ulist
print('dynamically coupled speeds:')
found = False
for i, r in enumerate(ulist):
for j, s in enumerate(ulist[i + 1:], i + 1):
if trigsimp(ic_matrix[i, j]) != 0:
found = True
print('{0} and {1}'.format(msprint(r), msprint(s)))
if not found:
print('None')
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
])
with warns_deprecated_sympy():
fr, fr_star = km.kanes_equations(forces, bodies)
assert (fr.expand() == fr_expected.expand())
assert ((fr_star_expected - trigsimp(fr_star)).expand() == zeros(3, 1))
lower_leg_mass, upper_leg_mass, torso_mass = symbols('m_L, m_U, m_T')
# Inertia
# =======
lower_leg_inertia, upper_leg_inertia, torso_inertia = symbols('I_Lz, I_Uz, I_Tz')
lower_leg_inertia_dyadic = inertia(lower_leg_frame, 0, 0, lower_leg_inertia)
lower_leg_central_inertia = (lower_leg_inertia_dyadic, lower_leg_mass_center)
upper_leg_inertia_dyadic = inertia(upper_leg_frame, 0, 0, upper_leg_inertia)
upper_leg_central_inertia = (upper_leg_inertia_dyadic, upper_leg_mass_center)
torso_inertia_dyadic = inertia(torso_frame, 0, 0, torso_inertia)
torso_central_inertia = (torso_inertia_dyadic, torso_mass_center)
# Rigid Bodies
# ============
lower_leg = RigidBody('Lower Leg', lower_leg_mass_center, lower_leg_frame,
lower_leg_mass, lower_leg_central_inertia)
upper_leg = RigidBody('Upper Leg', upper_leg_mass_center, upper_leg_frame,
upper_leg_mass, upper_leg_central_inertia)
torso = RigidBody('Torso', torso_mass_center, torso_frame,
torso_mass, torso_central_inertia)
vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R]
# Since S is rolling against C, v_S^_C = 0.
pO.set_vel(C, 0)
pS_star.v2pt_theory(pO, C, A)
pS_hat.v2pt_theory(pS_star, C, S)
vc += [dot(pS_hat.vel(C), basis) for basis in A]
# Cone has only angular velocity ω in R.z direction.
vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]]
vc += [omega - dot(C.ang_vel_in(R), R.z)]
vc_map = solve(vc, u)
# cone rigidbody
I_C = inertia(A, I11, I22, J)
rbC = RigidBody('rbC', pO, C, M, (I_C, pO))
# sphere rigidbody
I_S = inertia(A, 2 * m * r**2 / 5, 2 * m * r**2 / 5, 2 * m * r**2 / 5)
rbS = RigidBody('rbS', pS_star, S, m, (I_S, pS_star))
# kinetic energy
K = radsimp(
expand((rbC.kinetic_energy(R) + 4 * rbS.kinetic_energy(R)).subs(vc_map)))
print('K = {0}'.format(msprint(collect(K, omega**2 / 2))))
K_expected = (J + 18 * m * r**2 * (2 + sqrt(3)) / 5) * omega**2 / 2
#print('K_expected = {0}'.format(msprint(collect(expand(K_expected),
# omega**2/2))))
assert expand(K - K_expected) == 0
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
q1dd, q2dd = dynamicsymbols('q1 q2', 2)
m, Ia, It = symbols('m Ia It', real=True, positive=True)
N = ReferenceFrame('N')
F = N.orientnew('F', 'axis', [q1, N.z]) # gimbal frame
B = F.orientnew('B', 'axis', [q2, F.x]) # flywheel frame
P = Point('P')
P.set_vel(N, 0)
P.set_vel(F, 0)
P.set_vel(B, 0)
I = inertia(F, Ia, It, It, 0, 0, 0)
rb = RigidBody('flywheel', P, B, m, (I, P))
H = rb.angular_momentum(P, N)
print('H_P_N = {}\n = {}'.format(H.express(N), H.express(F)))
dH = H.dt(N)
print('d^N(H_P_N)/dt = {}'.format(dH.express(F).subs(q2dd, 0)))
print('\ndH/dt = M')
print('M = {}'.format(dH.express(F).subs(q2dd, 0)))
print('\ncalculation using euler angles')
t = symbols('t')
omega = F.ang_vel_in(N)
wx = omega & F.x
wy = omega & F.y
wz = omega & F.z
RBC_central_inertia = (RBC_dyadic,CoM_RBC)
LUB_central_inertia = (LUB_dyadic,CoM_LUB)
RUB_central_inertia = (RUB_dyadic,CoM_RUB)
LLB_central_inertia = (LLB_dyadic,CoM_LLB)
RLB_central_inertia = (RLB_dyadic,CoM_RLB)
Jbar_central_inertia = (Jbar_dyadic,CoM_Jbar)
LFT_central_inertia = (LFT_dyadic,CoM_LFT)
RFT_central_inertia = (RFT_dyadic,CoM_RFT)
LRT_central_inertia = (LRT_dyadic,CoM_LRT)
RRT_central_inertia = (RRT_dyadic,CoM_RRT)
# Rigid Bodies
# To completely define a rigid body, the mass center point, the reference frame,
# the mass, and the inertia defined about a point must be specified
Chassis = RigidBody('Chassis',CoM_Chassis,Chassis_frame,m_Chassis,Chassis_central_inertia)
RP = RigidBody('RP',CoM_RP,RP_frame,m_RP,RP_central_inertia)
LUCA = RigidBody('LUCA',CoM_LUCA,LUCA_frame,m_LUCA,LUCA_central_inertia)
RUCA = RigidBody('RUCA',CoM_RUCA,RUCA_frame,m_RUCA,RUCA_central_inertia)
LLCA = RigidBody('LLCA',CoM_LLCA,LLCA_frame,m_LLCA,LLCA_central_inertia)
RLCA = RigidBody('RLCA',CoM_RLCA,RLCA_frame,m_RLCA,RLCA_central_inertia)
LSpdl = RigidBody('LSpdl',CoM_LSpdl,LSpdl_frame,m_LSpdl,LSpdl_central_inertia)
RSpdl = RigidBody('RSpdl',CoM_RSpdl,RSpdl_frame,m_RSpdl,RSpdl_central_inertia)
LTR = RigidBody('LTR',CoM_LTR,LTR_frame,m_LTR,LTR_central_inertia)
RTR = RigidBody('RTR',CoM_RTR,RTR_frame,m_RTR,RTR_central_inertia)
Hsg = RigidBody('Hsg',CoM_Hsg,Hsg_frame,m_Hsg,Hsg_central_inertia)
LBC = RigidBody('LBC',CoM_LBC,LBC_frame,m_LBC,LBC_central_inertia)
RBC = RigidBody('RBC',CoM_RBC,RBC_frame,m_RBC,RBC_central_inertia)
LUB = RigidBody('LUB',CoM_LUB,LUB_frame,m_LUB,LUB_central_inertia)
RUB = RigidBody('RUB',CoM_RUB,RUB_frame,m_RUB,RUB_central_inertia)
LLB = RigidBody('LLB',CoM_LLB,LLB_frame,m_LLB,LLB_central_inertia)
upper_leg_right_inertia_dyadic = inertia(upper_leg_right_frame,
upper_leg_inertia, upper_leg_inertia,
upper_leg_inertia)
upper_leg_right_central_inertia = (upper_leg_right_inertia_dyadic,
upper_leg_right_mass_center)
upper_leg_right_inertia_dyadic.to_matrix(upper_leg_right_frame)
lower_leg_right_inertia_dyadic = inertia(lower_leg_right_frame,
lower_leg_inertia, lower_leg_inertia,
lower_leg_inertia)
lower_leg_right_central_inertia = (lower_leg_right_inertia_dyadic,
lower_leg_right_mass_center)
lower_leg_right_inertia_dyadic.to_matrix(lower_leg_right_frame)
lower_leg_left = RigidBody('Lower Leg Left', lower_leg_left_mass_center,
lower_leg_left_frame, lower_leg_mass,
lower_leg_left_central_inertia)
upper_leg_left = RigidBody('Upper Leg Left', upper_leg_left_mass_center,
upper_leg_left_frame, upper_leg_mass,
upper_leg_left_central_inertia)
hip = RigidBody('Hip', hip_mass_center, hip_frame, hip_mass,
hip_central_inertia)
upper_leg_right = RigidBody('Upper Leg Right', upper_leg_right_mass_center,
upper_leg_right_frame, upper_leg_mass,
upper_leg_right_central_inertia)
lower_leg_right = RigidBody('Lower Leg Right', lower_leg_right_mass_center,
lower_leg_right_frame, lower_leg_mass,
# velocity of the disk at the point of contact with the ground is not moving
# since the disk rolls without slipping.
pA = Point('pA') # ball bearing A
pB = pA.locatenew('pB', -R * F1.y) # ball bearing B
pA.set_vel(N, 0)
pA.set_vel(F1, 0)
pB.set_vel(F1, 0)
pB.set_vel(B, 0)
pB.v2pt_theory(pA, N, F1)
#pC.v2pt_theory(pB, N, B)
#print('\nvelocity of point C in N, v_C_N, at q1 = 0 = ')
#print(pC.vel(N).express(N).subs(q2d, q2d_val))
Ixx = m * r**2 / 4
Iyy = m * r**2 / 4
Izz = m * r**2 / 2
I_disc = inertia(B, Ixx, Iyy, Izz, 0, 0, 0)
rb_disc = RigidBody('Disc', pB, B, m, (I_disc, pB))
#T = rb_disc.kinetic_energy(N).subs({q2d: q2d_val}).subs({theta: theta_val})
T = rb_disc.kinetic_energy(N).subs({q2d: q2d_val})
print('T = {}'.format(msprint(simplify(T))))
values = {R: 1, r: 1, m: 0.5, theta: theta_val}
q1d_val = solve([-1 - q2d_val], q1d)[q1d]
#print(msprint(q1d_val))
print('T = {}'.format(msprint(simplify(T.subs(q1d, q1d_val).subs(values)))))
izx=0.00)
j1_central_inertia = (j1_inertia_dyadic, j1_mass_center)
#pretty_print(j1_inertia_dyadic.to_matrix(j1_frame))
j2_inertia_dyadic = inertia(j2_frame,
0.018,
0.001,
0.018,
ixy=0.000,
iyz=0.000,
izx=0.00)
j2_central_inertia = (j2_inertia_dyadic, j2_mass_center)
#pretty_print(j2_inertia_dyadic.to_matrix(j2_frame))
#fully define rigid bodies
link0 = RigidBody('Link 0', j0_mass_center, j0_frame, j0_mass,
j0_central_inertia)
link1 = RigidBody('Link 1', j1_mass_center, j1_frame, j1_mass,
j1_central_inertia)
link2 = RigidBody('Link 2', j2_mass_center, j2_frame, j2_mass,
j2_central_inertia)
print("finished Inertia")
#Kinetics---------------------------------------------------------------
g = symbols('g')
#ASSUMES GRAVITY BEING COMPENSATED BY ROBOT
#exert forces at a point
#j0_grav_force = (j0_mass_center, -j0_mass * g * inertial_frame.y)
j0_grav_force = (j0_mass_center, 0 * inertial_frame.y) #perp to dir of grav
j1_grav_force = (j1_mass_center, -j1_mass * g * inertial_frame.y)
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
}
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])
# 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)
K = mB*R**2/4
J = mB*R**2/2
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))
bodies = [rbA, rbB, rbC]
system = [i.masscenter for i in bodies] + [i.frame for i in bodies]
partials = partial_velocities(system, [u1, u2], F, kde_map, vc_map)
M = trigsimp(inertia_coefficient_matrix(bodies, partials))
print('inertia_coefficient_matrix = {0}'.format(msprint(M)))
M_expected = Matrix([[IA + mA*a**2 + mB*(R**2/2 + 3*b**2), 0],
[0, mA + 3*mB]])
assert expand(M - M_expected) == Matrix.zeros(2)
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}
from sympy.physics.mechanics import dot, dynamicsymbols, inertia, msprint
m, B11, B22, B33, B12, B23, B31 = symbols('m B11 B22 B33 B12 B23 B31')
q1, q2, q3, q4, q5, q6 = dynamicsymbols('q1:7')
# reference frames
A = ReferenceFrame('A')
B = A.orientnew('B', 'body', [q4, q5, q6], 'xyz')
omega = B.ang_vel_in(A)
# points B*, O
pB_star = Point('B*')
pO = pB_star.locatenew('O', q1*B.x + q2*B.y + q3*B.z)
pO.set_vel(A, 0)
pO.set_vel(B, 0)
pB_star.v2pt_theory(pO, A, B)
# rigidbody B
I_B_O = inertia(B, B11, B22, B33, B12, B23, B31)
rbB = RigidBody('rbB', pB_star, B, m, (I_B_O, pO))
# kinetic energy
K = rbB.kinetic_energy(A)
print('K = {0}'.format(msprint(trigsimp(K))))
K_expected = dot(dot(omega, I_B_O), omega)/2
print('K_expected = 1/2*omega*I*omega = {0}'.format(
msprint(trigsimp(K_expected))))
assert expand(expand_trig(K - K_expected)) == 0
