def __init__(self):
#We define some quantities required for tests here..
self.p = dynamicsymbols('p:3')
self.q = dynamicsymbols('q:3')
self.dynamic = list(self.p) + list(self.q)
self.states = [radians(45) for x in self.p] + \
[radians(30) for x in self.q]
self.I = ReferenceFrame('I')
self.A = self.I.orientnew('A', 'space', self.p, 'XYZ')
self.B = self.A.orientnew('B', 'space', self.q, 'XYZ')
self.O = Point('O')
self.P1 = self.O.locatenew('P1', 10 * self.I.x + \
10 * self.I.y + 10 * self.I.z)
self.P2 = self.P1.locatenew('P2', 10 * self.I.x + \
10 * self.I.y + 10 * self.I.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()
self.shape2 = Cylinder()
self.Ixx, self.Iyy, self.Izz = symbols('Ixx Iyy Izz')
self.mass = symbols('mass')
self.parameters = [self.Ixx, self.Iyy, self.Izz, self.mass]
self.param_vals = [0, 0, 0, 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.I, self.O, \
(self.global_frame1, self.global_frame2), \
name='scene')
self.particle = Particle('particle1', self.P1, self.mass)
#To make it more readable
p = self.p
q = self.q
#Here is the dragon ..
self.transformation_matrix = \
[[cos(p[1])*cos(p[2]), sin(p[2])*cos(p[1]), -sin(p[1]), 0], \
[sin(p[0])*sin(p[1])*cos(p[2]) - sin(p[2])*cos(p[0]), \
sin(p[0])*sin(p[1])*sin(p[2]) + cos(p[0])*cos(p[2]), \
sin(p[0])*cos(p[1]), 0], \
[sin(p[0])*sin(p[2]) + sin(p[1])*cos(p[0])*cos(p[2]), \
-sin(p[0])*cos(p[2]) + sin(p[1])*sin(p[2])*cos(p[0]), \
cos(p[0])*cos(p[1]), 0], \
[10, 10, 10, 1]]
def test_parallel_axis():
# This is for a 2 dof inverted pendulum on a cart.
# This tests the parallel axis code in Kane. The inertia of the pendulum is
# defined about the hinge, not about the center of mass.
# Defining the constants and knowns of the system
gravity = symbols('g')
k, ls = symbols('k ls')
a, mA, mC = symbols('a mA mC')
F = dynamicsymbols('F')
Ix, Iy, Iz = symbols('Ix Iy Iz')
# Declaring the Generalized coordinates and speeds
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
u1, u2 = dynamicsymbols('u1 u2')
u1d, u2d = dynamicsymbols('u1 u2', 1)
# Creating reference frames
N = ReferenceFrame('N')
A = ReferenceFrame('A')
A.orient(N, 'Axis', [-q2, N.z])
A.set_ang_vel(N, -u2 * N.z)
# Origin of Newtonian reference frame
O = Point('O')
# Creating and Locating the positions of the cart, C, and the
# center of mass of the pendulum, A
C = O.locatenew('C', q1 * N.x)
Ao = C.locatenew('Ao', a * A.y)
# Defining velocities of the points
O.set_vel(N, 0)
C.set_vel(N, u1 * N.x)
Ao.v2pt_theory(C, N, A)
Cart = Particle('Cart', C, mC)
Pendulum = RigidBody('Pendulum', Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))
# kinematical differential equations
kindiffs = [q1d - u1, q2d - u2]
bodyList = [Cart, Pendulum]
forceList = [(Ao, -N.y * gravity * mA),
(C, -N.y * gravity * mC),
(C, -N.x * k * (q1 - ls)),
(C, N.x * F)]
km=Kane(N)
km.coords([q1, q2])
km.speeds([u1, u2])
km.kindiffeq(kindiffs)
(fr,frstar) = km.kanes_equations(forceList, bodyList)
mm = km.mass_matrix_full
assert mm[3, 3] == -Iz
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 Kane. 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])
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)))
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 = [q1d - u3/cos(q3), q2d - u1, q3d - u2 + u3 * tan(q2)]
ForceList = [(Dmc, - m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
BodyD = RigidBody()
BodyD.mc = Dmc
BodyD.inertia = (I, Dmc)
BodyD.frame = R
BodyD.mass = m
BodyList = [BodyD]
KM = Kane(N)
KM.coords([q1, q2, q3])
KM.speeds([u1, u2, u3, u4, u5])
KM.kindiffeq(kd)
kdd = KM.kindiffdict()
(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 = Kane(N)
KM2.coords([q1, q2, q3])
KM2.speeds([u1, u2, u3], u_auxiliary=[u4, u5])
KM2.kindiffeq(kd)
(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})
assert fr.expand() == fr2.expand()
assert frstar.expand() == frstar2.expand()
def test_inertia():
N = ReferenceFrame('N')
ixx, iyy, izz = symbols('ixx iyy izz')
ixy, iyz, izx = symbols('ixy iyz izx')
assert inertia(N, ixx, iyy, izz) == (ixx * (N.x | N.x) + iyy *
(N.y | N.y) + izz * (N.z | N.z))
assert inertia(N, 0, 0, 0) == 0 * (N.x | N.x)
assert inertia(N, ixx, iyy, izz, ixy, iyz, izx) == (ixx * (N.x | N.x) +
ixy * (N.x | N.y) + izx * (N.x | N.z) + ixy * (N.y | N.x) + iyy *
(N.y | N.y) + iyz * (N.y | N.z) + izx * (N.z | N.x) + iyz * (N.z |
N.y) + izz * (N.z | N.z))
def _create_inertia_dyadics(self):
leg_inertia_dyadic = me.inertia(self.frames['leg'], 0, 0,
self.parameters['leg_inertia'])
torso_inertia_dyadic = me.inertia(self.frames['torso'], 0, 0,
self.parameters['torso_inertia'])
self.central_inertias = OrderedDict()
self.central_inertias['leg'] = (leg_inertia_dyadic,
self.points['leg_mass_center'])
self.central_inertias['torso'] = (torso_inertia_dyadic,
self.points['torso_mass_center'])
def test_specifying_coordinate_issue_339():
"""This test ensures that you can use derivatives as specified values."""
# beta will be a specified angle
beta = me.dynamicsymbols('beta')
q1, q2, q3, q4 = me.dynamicsymbols('q1, q2, q3, q4')
u1, u2, u3, u4 = me.dynamicsymbols('u1, u2, u3, u4')
N = me.ReferenceFrame('N')
A = N.orientnew('A', 'Axis', (q1, N.x))
B = A.orientnew('B', 'Axis', (beta, A.y))
No = me.Point('No')
Ao = No.locatenew('Ao', q2 * N.x + q3 * N.y + q4 * N.z)
Bo = Ao.locatenew('Bo', 10 * A.x + 10 * A.y + 10 * A.z)
A.set_ang_vel(N, u1 * N.x)
B.ang_vel_in(N) # compute it automatically
No.set_vel(N, 0)
Ao.set_vel(N, u2 * N.x + u3 * N.y + u4 * N.z)
Bo.v2pt_theory(Ao, N, B)
body_A = me.RigidBody('A', Ao, A, 1.0, (me.inertia(A, 1, 2, 3), Ao))
body_B = me.RigidBody('B', Bo, B, 1.0, (me.inertia(A, 3, 2, 1), Bo))
bodies = [body_A, body_B]
# TODO : This should be able to be simple an empty iterable.
loads = [(No, 0 * N.x)]
kdes = [u1 - q1.diff(),
u2 - q2.diff(),
u3 - q3.diff(),
u4 - q4.diff()]
kane = me.KanesMethod(N, q_ind=[q1, q2, q3, q4],
u_ind=[u1, u2, u3, u4], kd_eqs=kdes)
if sympy_newer_than('1.0'):
fr, frstar = kane.kanes_equations(bodies, loads)
else:
fr, frstar = kane.kanes_equations(loads, bodies)
sys = System(kane)
sys.specifieds = {(beta, beta.diff(), beta.diff().diff()):
lambda x, t: np.array([1.0, 1.0, 1.0])}
sys.times = np.linspace(0, 10, 20)
sys.integrate()
def test_body_add_force():
# Body with RigidBody.
rigidbody_masscenter = Point('rigidbody_masscenter')
rigidbody_mass = Symbol('rigidbody_mass')
rigidbody_frame = ReferenceFrame('rigidbody_frame')
body_inertia = inertia(rigidbody_frame, 1, 0, 0)
rigid_body = Body('rigidbody_body', rigidbody_masscenter, rigidbody_mass,
rigidbody_frame, body_inertia)
l = Symbol('l')
Fa = Symbol('Fa')
point = rigid_body.masscenter.locatenew(
'rigidbody_body_point0',
l * rigid_body.frame.x)
point.set_vel(rigid_body.frame, 0)
force_vector = Fa * rigid_body.frame.z
# apply_force with point
rigid_body.apply_force(force_vector, point)
assert len(rigid_body.loads) == 1
force_point = rigid_body.loads[0][0]
frame = rigid_body.frame
assert force_point.vel(frame) == point.vel(frame)
assert force_point.pos_from(force_point) == point.pos_from(force_point)
assert rigid_body.loads[0][1] == force_vector
# apply_force without point
rigid_body.apply_force(force_vector)
assert len(rigid_body.loads) == 2
assert rigid_body.loads[1][1] == force_vector
# passing something else than point
raises(TypeError, lambda: rigid_body.apply_force(force_vector, 0))
raises(TypeError, lambda: rigid_body.apply_force(0))
def test_body_add_force():
# Body with RigidBody.
rigidbody_masscenter = Point('rigidbody_masscenter')
rigidbody_mass = Symbol('rigidbody_mass')
rigidbody_frame = ReferenceFrame('rigidbody_frame')
body_inertia = inertia(rigidbody_frame, 1, 0, 0)
rigid_body = Body('rigidbody_body', rigidbody_masscenter, rigidbody_mass,
rigidbody_frame, body_inertia)
l = Symbol('l')
Fa = Symbol('Fa')
point = rigid_body.masscenter.locatenew('rigidbody_body_point0',
l * rigid_body.frame.x)
point.set_vel(rigid_body.frame, 0)
force_vector = Fa * rigid_body.frame.z
# apply_force with point
rigid_body.apply_force(force_vector, point)
assert len(rigid_body.loads) == 1
force_point = rigid_body.loads[0][0]
frame = rigid_body.frame
assert force_point.vel(frame) == point.vel(frame)
assert force_point.pos_from(force_point) == point.pos_from(force_point)
assert rigid_body.loads[0][1] == force_vector
# apply_force without point
rigid_body.apply_force(force_vector)
assert len(rigid_body.loads) == 2
assert rigid_body.loads[1][1] == force_vector
# passing something else than point
raises(TypeError, lambda: rigid_body.apply_force(force_vector, 0))
raises(TypeError, lambda: rigid_body.apply_force(0))
def test_custom_rigid_body():
# Body with RigidBody.
rigidbody_masscenter = Point("rigidbody_masscenter")
rigidbody_mass = Symbol("rigidbody_mass")
rigidbody_frame = ReferenceFrame("rigidbody_frame")
body_inertia = inertia(rigidbody_frame, 1, 0, 0)
rigid_body = Body(
"rigidbody_body",
rigidbody_masscenter,
rigidbody_mass,
rigidbody_frame,
body_inertia,
)
com = rigid_body.masscenter
frame = rigid_body.frame
rigidbody_masscenter.set_vel(rigidbody_frame, 0)
assert com.vel(frame) == rigidbody_masscenter.vel(frame)
assert com.pos_from(com) == rigidbody_masscenter.pos_from(com)
assert rigid_body.mass == rigidbody_mass
assert rigid_body.inertia == (body_inertia, rigidbody_masscenter)
assert hasattr(rigid_body, "masscenter")
assert hasattr(rigid_body, "mass")
assert hasattr(rigid_body, "frame")
assert hasattr(rigid_body, "inertia")
def test_pendulum_angular_momentum():
"""Consider a pendulum of length OA = 2a, of mass m as a rigid body of
center of mass G (OG = a) which turn around (O,z). The angle between the
reference frame R and the rod is q. The inertia of the body is I =
(G,0,ma^2/3,ma^2/3). """
m, a = symbols('m, a')
q = dynamicsymbols('q')
R = ReferenceFrame('R')
R1 = R.orientnew('R1', 'Axis', [q, R.z])
R1.set_ang_vel(R, q.diff() * R.z)
I = inertia(R1, 0, m * a**2 / 3, m * a**2 / 3)
O = Point('O')
A = O.locatenew('A', 2*a * R1.x)
G = O.locatenew('G', a * R1.x)
S = RigidBody('S', G, R1, m, (I, G))
O.set_vel(R, 0)
A.v2pt_theory(O, R, R1)
G.v2pt_theory(O, R, R1)
assert (4 * m * a**2 / 3 * q.diff() * R.z -
S.angular_momentum(O, R).express(R)) == 0
def test_pendulum_angular_momentum():
"""Consider a pendulum of length OA = 2a, of mass m as a rigid body of
center of mass G (OG = a) which turn around (O,z). The angle between the
reference frame R and the rod is q. The inertia of the body is I =
(G,0,ma^2/3,ma^2/3). """
m, a = symbols('m, a')
q = dynamicsymbols('q')
R = ReferenceFrame('R')
R1 = R.orientnew('R1', 'Axis', [q, R.z])
R1.set_ang_vel(R, q.diff() * R.z)
I = inertia(R1, 0, m * a**2 / 3, m * a**2 / 3)
O = Point('O')
A = O.locatenew('A', 2 * a * R1.x)
G = O.locatenew('G', a * R1.x)
S = RigidBody('S', G, R1, m, (I, G))
O.set_vel(R, 0)
A.v2pt_theory(O, R, R1)
G.v2pt_theory(O, R, R1)
assert (4 * m * a**2 / 3 * q.diff() * R.z -
S.angular_momentum(O, R).express(R)) == 0
def __init__(self):
#We define some quantities required for tests here..
self.p = dynamicsymbols('p:3')
self.q = dynamicsymbols('q:3')
self.dynamic = list(self.p) + list(self.q)
self.states = [radians(45) for x in self.p] + \
[radians(30) for x in self.q]
self.I = ReferenceFrame('I')
self.A = self.I.orientnew('A', 'space', self.p, 'XYZ')
self.B = self.A.orientnew('B', 'space', self.q, 'XYZ')
self.O = Point('O')
self.P1 = self.O.locatenew('P1', 10 * self.I.x + \
10 * self.I.y + 10 * self.I.z)
self.P2 = self.P1.locatenew('P2', 10 * self.I.x + \
10 * self.I.y + 10 * self.I.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)
self.Ixx, self.Iyy, self.Izz = symbols('Ixx Iyy Izz')
self.mass = symbols('mass')
self.parameters = [self.Ixx, self.Iyy, self.Izz, self.mass]
self.param_vals = [0, 0, 0, 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.I, self.O, \
(self.global_frame1, self.global_frame2), \
name='scene')
self.particle = Particle('particle1', self.P1, self.mass)
#To make it more readable
p = self.p
q = self.q
#Here is the dragon ..
self.transformation_matrix = \
[[cos(p[1])*cos(p[2]), sin(p[2])*cos(p[1]), -sin(p[1]), 0], \
[sin(p[0])*sin(p[1])*cos(p[2]) - sin(p[2])*cos(p[0]), \
sin(p[0])*sin(p[1])*sin(p[2]) + cos(p[0])*cos(p[2]), \
sin(p[0])*cos(p[1]), 0], \
[sin(p[0])*sin(p[2]) + sin(p[1])*cos(p[0])*cos(p[2]), \
-sin(p[0])*cos(p[2]) + sin(p[1])*sin(p[2])*cos(p[0]), \
cos(p[0])*cos(p[1]), 0], \
[10, 10, 10, 1]]
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 test_parallel_axis():
# This is for a 2 dof inverted pendulum on a cart.
# This tests the parallel axis code in KanesMethod. The inertia of the
# pendulum is defined about the hinge, not about the center of mass.
# Defining the constants and knowns of the system
gravity = symbols("g")
k, ls = symbols("k ls")
a, mA, mC = symbols("a mA mC")
F = dynamicsymbols("F")
Ix, Iy, Iz = symbols("Ix Iy Iz")
# Declaring the Generalized coordinates and speeds
q1, q2 = dynamicsymbols("q1 q2")
q1d, q2d = dynamicsymbols("q1 q2", 1)
u1, u2 = dynamicsymbols("u1 u2")
u1d, u2d = dynamicsymbols("u1 u2", 1)
# Creating reference frames
N = ReferenceFrame("N")
A = ReferenceFrame("A")
A.orient(N, "Axis", [-q2, N.z])
A.set_ang_vel(N, -u2 * N.z)
# Origin of Newtonian reference frame
O = Point("O")
# Creating and Locating the positions of the cart, C, and the
# center of mass of the pendulum, A
C = O.locatenew("C", q1 * N.x)
Ao = C.locatenew("Ao", a * A.y)
# Defining velocities of the points
O.set_vel(N, 0)
C.set_vel(N, u1 * N.x)
Ao.v2pt_theory(C, N, A)
Cart = Particle("Cart", C, mC)
Pendulum = RigidBody("Pendulum", Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))
# kinematical differential equations
kindiffs = [q1d - u1, q2d - u2]
bodyList = [Cart, Pendulum]
forceList = [
(Ao, -N.y * gravity * mA),
(C, -N.y * gravity * mC),
(C, -N.x * k * (q1 - ls)),
(C, N.x * F),
]
km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs)
with warns_deprecated_sympy():
(fr, frstar) = km.kanes_equations(forceList, bodyList)
mm = km.mass_matrix_full
assert mm[3, 3] == Iz
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 Kane. 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])
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)))
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 = [q1d - u3 / cos(q3), q2d - u1, q3d - u2 + u3 * tan(q2)]
ForceList = [(Dmc, -m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
BodyD = RigidBody()
BodyD.mc = Dmc
BodyD.inertia = (I, Dmc)
BodyD.frame = R
BodyD.mass = m
BodyList = [BodyD]
KM = Kane(N)
KM.coords([q1, q2, q3])
KM.speeds([u1, u2, u3, u4, u5])
KM.kindiffeq(kd)
kdd = KM.kindiffdict()
(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 = Kane(N)
KM2.coords([q1, q2, q3])
KM2.speeds([u1, u2, u3], u_auxiliary=[u4, u5])
KM2.kindiffeq(kd)
(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})
assert fr.expand() == fr2.expand()
assert frstar.expand() == frstar2.expand()
def test_parallel_axis():
# This is for a 2 dof inverted pendulum on a cart.
# This tests the parallel axis code in KanesMethod. The inertia of the
# pendulum is defined about the hinge, not about the center of mass.
# Defining the constants and knowns of the system
gravity = symbols('g')
k, ls = symbols('k ls')
a, mA, mC = symbols('a mA mC')
F = dynamicsymbols('F')
Ix, Iy, Iz = symbols('Ix Iy Iz')
# Declaring the Generalized coordinates and speeds
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
u1, u2 = dynamicsymbols('u1 u2')
u1d, u2d = dynamicsymbols('u1 u2', 1)
# Creating reference frames
N = ReferenceFrame('N')
A = ReferenceFrame('A')
A.orient(N, 'Axis', [-q2, N.z])
A.set_ang_vel(N, -u2 * N.z)
# Origin of Newtonian reference frame
O = Point('O')
# Creating and Locating the positions of the cart, C, and the
# center of mass of the pendulum, A
C = O.locatenew('C', q1 * N.x)
Ao = C.locatenew('Ao', a * A.y)
# Defining velocities of the points
O.set_vel(N, 0)
C.set_vel(N, u1 * N.x)
Ao.v2pt_theory(C, N, A)
Cart = Particle('Cart', C, mC)
Pendulum = RigidBody('Pendulum', Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))
# kinematical differential equations
kindiffs = [q1d - u1, q2d - u2]
bodyList = [Cart, Pendulum]
forceList = [(Ao, -N.y * gravity * mA),
(C, -N.y * gravity * mC),
(C, -N.x * k * (q1 - ls)),
(C, N.x * F)]
km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar) = km.kanes_equations(forceList, bodyList)
mm = km.mass_matrix_full
assert mm[3, 3] == Iz
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 3
# speed variables are need to describe this system, along with the
# disc's mass and radius, and the local gravity.
q1, q2, q3 = dynamicsymbols('q1 q2 q3')
q1d, q2d, q3d = dynamicsymbols('q1 q2 q3', 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])
# 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)
# Forming the inertia dyadic.
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
# Finally we form the equations of motion, using the same steps we did
# before. Supply the Lagrangian, the generalized speeds.
BodyD.potential_energy = -m * g * r * cos(q2)
Lag = Lagrangian(N, BodyD)
q = [q1, q2, q3]
q1 = Function('q1')
q2 = Function('q2')
q3 = Function('q3')
l = LagrangesMethod(Lag, q)
l.form_lagranges_equations()
RHS = l.rhs().as_mutable()
RHS.simplify()
t = symbols('t')
assert tuple(l.mass_matrix[3:6]) == (0, 5 * m * r**2 / 4, 0)
assert RHS[4].simplify() == (
(-8 * g * sin(q2(t)) + r *
(5 * sin(2 * q2(t)) * Derivative(q1(t), t) +
12 * cos(q2(t)) * Derivative(q3(t), t)) * Derivative(q1(t), t)) /
(10 * r))
assert RHS[5] == (-5 * cos(q2(t)) * Derivative(q1(t), t) +
6 * tan(q2(t)) * Derivative(q3(t), t) +
4 * Derivative(q1(t), t) / cos(q2(t))) * Derivative(
q2(t), t)
def test_parallel_axis():
N = ReferenceFrame('N')
m, a, b = symbols('m, a, b')
o = Point('o')
p = o.locatenew('p', a * N.x + b * N.y)
P = Particle('P', o, m)
Ip = P.parallel_axis(p, N)
Ip_expected = inertia(N, m * b**2, m * a**2, m * (a**2 + b**2),
ixy=-m * a * b)
assert Ip == Ip_expected
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 3
# speed variables are need to describe this system, along with the
# disc's mass and radius, and the local gravity.
q1, q2, q3 = dynamicsymbols('q1 q2 q3')
q1d, q2d, q3d = dynamicsymbols('q1 q2 q3', 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])
# 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)
# Forming the inertia dyadic.
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
# Finally we form the equations of motion, using the same steps we did
# before. Supply the Lagrangian, the generalized speeds.
BodyD.set_potential_energy(- m * g * r * cos(q2))
Lag = Lagrangian(N, BodyD)
q = [q1, q2, q3]
q1 = Function('q1')
q2 = Function('q2')
q3 = Function('q3')
l = LagrangesMethod(Lag, q)
l.form_lagranges_equations()
RHS = l.rhs()
RHS.simplify()
t = symbols('t')
assert (l.mass_matrix[3:6] == [0, 5*m*r**2/4, 0])
assert RHS[4].simplify() == (-8*g*sin(q2(t)) + 5*r*sin(2*q2(t)
)*Derivative(q1(t), t)**2 + 12*r*cos(q2(t))*Derivative(q1(t), t
)*Derivative(q3(t), t))/(10*r)
assert RHS[5] == (-5*cos(q2(t))*Derivative(q1(t), t) + 6*tan(q2(t)
)*Derivative(q3(t), t) + 4*Derivative(q1(t), t)/cos(q2(t))
)*Derivative(q2(t), t)
def test_parallel_axis():
# This is for a 2 dof inverted pendulum on a cart.
# This tests the parallel axis code in KanesMethod. The inertia of the
# pendulum is defined about the hinge, not about the center of mass.
# Defining the constants and knowns of the system
gravity = symbols("g")
k, ls = symbols("k ls")
a, mA, mC = symbols("a mA mC")
F = dynamicsymbols("F")
Ix, Iy, Iz = symbols("Ix Iy Iz")
# Declaring the Generalized coordinates and speeds
q1, q2 = dynamicsymbols("q1 q2")
q1d, q2d = dynamicsymbols("q1 q2", 1)
u1, u2 = dynamicsymbols("u1 u2")
u1d, u2d = dynamicsymbols("u1 u2", 1)
# Creating reference frames
N = ReferenceFrame("N")
A = ReferenceFrame("A")
A.orient(N, "Axis", [-q2, N.z])
A.set_ang_vel(N, -u2 * N.z)
# Origin of Newtonian reference frame
O = Point("O")
# Creating and Locating the positions of the cart, C, and the
# center of mass of the pendulum, A
C = O.locatenew("C", q1 * N.x)
Ao = C.locatenew("Ao", a * A.y)
# Defining velocities of the points
O.set_vel(N, 0)
C.set_vel(N, u1 * N.x)
Ao.v2pt_theory(C, N, A)
Cart = Particle("Cart", C, mC)
Pendulum = RigidBody("Pendulum", Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))
# kinematical differential equations
kindiffs = [q1d - u1, q2d - u2]
bodyList = [Cart, Pendulum]
forceList = [(Ao, -N.y * gravity * mA), (C, -N.y * gravity * mC), (C, -N.x * k * (q1 - ls)), (C, N.x * F)]
km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar) = km.kanes_equations(forceList, bodyList)
mm = km.mass_matrix_full
assert mm[3, 3] == Iz
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 parallel_axis(self, point):
"""Returns the inertia of the body about another point."""
# TODO : What if the new point is not fixed in the rigid body's frame?
a, b, c = self.masscenter.pos_from(point).to_matrix(self.frame)
return self.central_inertia + self.mass * inertia(self.frame,
b**2 + c**2,
c**2 + a**2,
a**2 + b**2,
-a * b,
-b * c,
-a * c)
def test_parallel_axis():
N = ReferenceFrame("N")
m, a, b = symbols("m, a, b")
o = Point("o")
p = o.locatenew("p", a * N.x + b * N.y)
P = Particle("P", o, m)
Ip = P.parallel_axis(p, N)
Ip_expected = inertia(N,
m * b**2,
m * a**2,
m * (a**2 + b**2),
ixy=-m * a * b)
assert Ip == Ip_expected
def test_default():
body = Body('body')
assert body.name == 'body'
assert body.loads == []
point = Point('body_masscenter')
point.set_vel(body.frame, 0)
com = body.masscenter
frame = body.frame
assert com.vel(frame) == point.vel(frame)
assert body.mass == Symbol('body_mass')
ixx, iyy, izz = symbols('body_ixx body_iyy body_izz')
ixy, iyz, izx = symbols('body_ixy body_iyz body_izx')
assert body.inertia == (inertia(body.frame, ixx, iyy, izz, ixy, iyz,
izx), body.masscenter)
def test_default():
body = Body('body')
assert body.name == 'body'
assert body.loads == []
point = Point('body_masscenter')
point.set_vel(body.frame, 0)
com = body.masscenter
frame = body.frame
assert com.vel(frame) == point.vel(frame)
assert body.mass == Symbol('body_mass')
ixx, iyy, izz = symbols('body_ixx body_iyy body_izz')
ixy, iyz, izx = symbols('body_ixy body_iyz body_izx')
assert body.inertia == (inertia(body.frame, ixx, iyy, izz, ixy, iyz, izx),
body.masscenter)
def _create_inertia_dyadics(self):
lthigh_inertia_dyadic = me.inertia(self.frames['left_thigh'], 0, 0,
self.parameters['thigh_inertia'])
lshank_inertia_dyadic = me.inertia(self.frames['left_shank'], 0, 0,
self.parameters['shank_inertia'])
rthigh_inertia_dyadic = me.inertia(self.frames['right_thigh'], 0, 0,
self.parameters['thigh_inertia'])
rshank_inertia_dyadic = me.inertia(self.frames['right_shank'], 0, 0,
self.parameters['shank_inertia'])
self.central_inertias = OrderedDict()
self.central_inertias['lthigh'] = (lthigh_inertia_dyadic,
self.points['lthigh_mass_center'])
self.central_inertias['lshank'] = (lshank_inertia_dyadic,
self.points['lshank_mass_center'])
self.central_inertias['rthigh'] = (rthigh_inertia_dyadic,
self.points['rthigh_mass_center'])
self.central_inertias['rshank'] = (rshank_inertia_dyadic,
self.points['rshank_mass_center'])
def test_default():
body = Body("body")
assert body.name == "body"
assert body.loads == []
point = Point("body_masscenter")
point.set_vel(body.frame, 0)
com = body.masscenter
frame = body.frame
assert com.vel(frame) == point.vel(frame)
assert body.mass == Symbol("body_mass")
ixx, iyy, izz = symbols("body_ixx body_iyy body_izz")
ixy, iyz, izx = symbols("body_ixy body_iyz body_izx")
assert body.inertia == (
inertia(body.frame, ixx, iyy, izz, ixy, iyz, izx),
body.masscenter,
)
def test_linearize_rolling_disc_lagrange():
q1, q2, q3 = q = dynamicsymbols("q1 q2 q3")
q1d, q2d, q3d = qd = dynamicsymbols("q1 q2 q3", 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])
C = Point("C")
C.set_vel(N, 0)
Dmc = C.locatenew("Dmc", r * L.z)
Dmc.v2pt_theory(C, N, R)
I = inertia(L, m / 4 * r ** 2, m / 2 * r ** 2, m / 4 * r ** 2)
BodyD = RigidBody("BodyD", Dmc, R, m, (I, Dmc))
BodyD.potential_energy = -m * g * r * cos(q2)
Lag = Lagrangian(N, BodyD)
l = LagrangesMethod(Lag, q)
l.form_lagranges_equations()
# Linearize about steady-state upright rolling
op_point = {
q1: 0,
q2: 0,
q3: 0,
q1d: 0,
q2d: 0,
q1d.diff(): 0,
q2d.diff(): 0,
q3d.diff(): 0,
}
A = l.linearize(q_ind=q, qd_ind=qd, op_point=op_point, A_and_B=True)[0]
sol = Matrix(
[
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, -6 * q3d, 0],
[0, -4 * g / (5 * r), 0, 6 * q3d / 5, 0, 0],
[0, 0, 0, 0, 0, 0],
]
)
assert A == sol
def __init__(self,
name,
masscenter=None,
mass=None,
frame=None,
central_inertia=None):
self.name = name
self.loads = []
if frame is None:
frame = ReferenceFrame(name + '_frame')
if masscenter is None:
masscenter = Point(name + '_masscenter')
if central_inertia is None and mass is None:
ixx = Symbol(name + '_ixx')
iyy = Symbol(name + '_iyy')
izz = Symbol(name + '_izz')
izx = Symbol(name + '_izx')
ixy = Symbol(name + '_ixy')
iyz = Symbol(name + '_iyz')
_inertia = (inertia(frame, ixx, iyy, izz, ixy, iyz,
izx), masscenter)
else:
_inertia = (central_inertia, masscenter)
if mass is None:
_mass = Symbol(name + '_mass')
else:
_mass = mass
masscenter.set_vel(frame, 0)
# If user passes masscenter and mass then a particle is created
# otherwise a rigidbody. As a result a body may or may not have inertia.
if central_inertia is None and mass is not None:
self.frame = frame
self.masscenter = masscenter
Particle.__init__(self, name, masscenter, _mass)
else:
RigidBody.__init__(self, name, masscenter, frame, _mass, _inertia)
def test_custom_rigid_body():
# Body with RigidBody.
rigidbody_masscenter = Point('rigidbody_masscenter')
rigidbody_mass = Symbol('rigidbody_mass')
rigidbody_frame = ReferenceFrame('rigidbody_frame')
body_inertia = inertia(rigidbody_frame, 1, 0, 0)
rigid_body = Body('rigidbody_body', rigidbody_masscenter, rigidbody_mass,
rigidbody_frame, body_inertia)
com = rigid_body.masscenter
frame = rigid_body.frame
rigidbody_masscenter.set_vel(rigidbody_frame, 0)
assert com.vel(frame) == rigidbody_masscenter.vel(frame)
assert com.pos_from(com) == rigidbody_masscenter.pos_from(com)
assert rigid_body.mass == rigidbody_mass
assert rigid_body.inertia == (body_inertia, rigidbody_masscenter)
assert hasattr(rigid_body, 'masscenter')
assert hasattr(rigid_body, 'mass')
assert hasattr(rigid_body, 'frame')
assert hasattr(rigid_body, 'inertia')
def test_custom_rigid_body():
# Body with RigidBody.
rigidbody_masscenter = Point('rigidbody_masscenter')
rigidbody_mass = Symbol('rigidbody_mass')
rigidbody_frame = ReferenceFrame('rigidbody_frame')
body_inertia = inertia(rigidbody_frame, 1, 0, 0)
rigid_body = Body('rigidbody_body', rigidbody_masscenter, rigidbody_mass,
rigidbody_frame, body_inertia)
com = rigid_body.masscenter
frame = rigid_body.frame
rigidbody_masscenter.set_vel(rigidbody_frame, 0)
assert com.vel(frame) == rigidbody_masscenter.vel(frame)
assert com.pos_from(com) == rigidbody_masscenter.pos_from(com)
assert rigid_body.mass == rigidbody_mass
assert rigid_body.inertia == (body_inertia, rigidbody_masscenter)
assert hasattr(rigid_body, 'masscenter')
assert hasattr(rigid_body, 'mass')
assert hasattr(rigid_body, 'frame')
assert hasattr(rigid_body, 'inertia')
def __init__(self, name, masscenter=None, mass=None, frame=None,
central_inertia=None):
self.name = name
self.loads = []
if frame is None:
frame = ReferenceFrame(name + '_frame')
if masscenter is None:
masscenter = Point(name + '_masscenter')
if central_inertia is None and mass is None:
ixx = Symbol(name + '_ixx')
iyy = Symbol(name + '_iyy')
izz = Symbol(name + '_izz')
izx = Symbol(name + '_izx')
ixy = Symbol(name + '_ixy')
iyz = Symbol(name + '_iyz')
_inertia = (inertia(frame, ixx, iyy, izz, ixy, iyz, izx),
masscenter)
else:
_inertia = (central_inertia, masscenter)
if mass is None:
_mass = Symbol(name + '_mass')
else:
_mass = mass
masscenter.set_vel(frame, 0)
# If user passes masscenter and mass then a particle is created
# otherwise a rigidbody. As a result a body may or may not have inertia.
if central_inertia is None and mass is not None:
self.frame = frame
self.masscenter = masscenter
Particle.__init__(self, name, masscenter, _mass)
else:
RigidBody.__init__(self, name, masscenter, frame, _mass, _inertia)
def test_linearize_rolling_disc_lagrange():
q1, q2, q3 = q = dynamicsymbols("q1 q2 q3")
q1d, q2d, q3d = qd = dynamicsymbols("q1 q2 q3", 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])
C = Point("C")
C.set_vel(N, 0)
Dmc = C.locatenew("Dmc", r * L.z)
Dmc.v2pt_theory(C, N, R)
I = inertia(L, m / 4 * r ** 2, m / 2 * r ** 2, m / 4 * r ** 2)
BodyD = RigidBody("BodyD", Dmc, R, m, (I, Dmc))
BodyD.potential_energy = -m * g * r * cos(q2)
Lag = Lagrangian(N, BodyD)
l = LagrangesMethod(Lag, q)
l.form_lagranges_equations()
# Linearize about steady-state upright rolling
op_point = {q1: 0, q2: 0, q3: 0, q1d: 0, q2d: 0, q1d.diff(): 0, q2d.diff(): 0, q3d.diff(): 0}
A = l.linearize(q_ind=q, qd_ind=qd, op_point=op_point, A_and_B=True)[0]
sol = Matrix(
[
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, -6 * q3d, 0],
[0, -4 * g / (5 * r), 0, 6 * q3d / 5, 0, 0],
[0, 0, 0, 0, 0, 0],
]
)
assert A == sol
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()
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)
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}
l: 2,
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())))
# 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)
# 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]
# forces, torques
forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x + Q2*A.y + Q3*A.z)]
torques = []
# collect all significant points/frames of the system
system = [y for x in bodies for y in [x.masscenter, x.frame]]
origin=Point('o')
center=origin.locatenew('c',x*NewtonFrame.x\
+y*NewtonFrame.y+z*NewtonFrame.z)
front=center.locatenew('f',L*RobotFrame.x)
left=center.locatenew('l',L*RobotFrame.y)
back=center.locatenew('b',-L*RobotFrame.x)
right=center.locatenew('r',-L*RobotFrame.y)
motorPoints=[front,right,back,left]
# set velocities
origin.set_vel(NewtonFrame,0)
center.set_vel(NewtonFrame,xDot*NewtonFrame.x\
+yDot*NewtonFrame.y+zDot*NewtonFrame.z)
for point in motorPoints:
point.v2pt_theory(center,NewtonFrame,RobotFrame)
# inertia dyadic
Inertia=inertia(RobotFrame,m*L**2/2,m*L**2/2,m*L**2)
# quadcopter rigid body
Quadcopter=RigidBody('Q',center,RobotFrame,m,(Inertia,center))
# Generate Lists
ForceList=[(point,k*motor*motor*RobotFrame.z)\
for point,motor in zip(motorPoints,motorInputs)]
Torques=[b*motor*motor*RobotFrame.z for motor in motorInputs]
ForceList.append((center,-m*g*NewtonFrame.z))
Torques[1]=-Torques[1];Torques[3]=-Torques[3]
TorqueList=Torques
# Newton's Method
state=symbols(dofNames)
velocity=symbols(['d'+name for name in dofNames])
acceleration=symbols(['dd'+name for name in dofNames])
diff_velocity = [diff(vel) for vel in velocity]
symdict=dict(zip(q+qd+qdd,state+velocity+acceleration))
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
}
import sympy.physics.mechanics as _me
import sympy as _sm
import math as m
import numpy as _np
frame_a = _me.ReferenceFrame('a')
c1, c2, c3 = _sm.symbols('c1 c2 c3', real=True)
a = _me.inertia(frame_a, 1, 1, 1)
particle_p1 = _me.Particle('p1', _me.Point('p1_pt'), _sm.Symbol('m'))
particle_p2 = _me.Particle('p2', _me.Point('p2_pt'), _sm.Symbol('m'))
body_r_cm = _me.Point('r_cm')
body_r_f = _me.ReferenceFrame('r_f')
body_r = _me.RigidBody('r', body_r_cm, body_r_f, _sm.symbols('m'), (_me.outer(body_r_f.x,body_r_f.x),body_r_cm))
frame_a.orient(body_r_f, 'DCM', _sm.Matrix([1,1,1,1,1,0,0,0,1]).reshape(3, 3))
point_o = _me.Point('o')
m1 = _sm.symbols('m1')
particle_p1.mass = m1
m2 = _sm.symbols('m2')
particle_p2.mass = m2
mr = _sm.symbols('mr')
body_r.mass = mr
i1 = _sm.symbols('i1')
i2 = _sm.symbols('i2')
i3 = _sm.symbols('i3')
body_r.inertia = (_me.inertia(body_r_f, i1, i2, i3, 0, 0, 0), body_r_cm)
point_o.set_pos(particle_p1.point, c1*frame_a.x)
point_o.set_pos(particle_p2.point, c2*frame_a.y)
point_o.set_pos(body_r_cm, c3*frame_a.z)
a = _me.inertia_of_point_mass(particle_p1.mass, particle_p1.point.pos_from(point_o), frame_a)
a = _me.inertia_of_point_mass(particle_p2.mass, particle_p2.point.pos_from(point_o), frame_a)
a = body_r.inertia[0] + _me.inertia_of_point_mass(body_r.mass, body_r.masscenter.pos_from(point_o), frame_a)
q = dynamicsymbols('q:5') # Generalized coordinates
qd = [qi.diff(t) for qi in q] # Generalized coordinate time derivatives
u = dynamicsymbols('u:3') # Generalized speeds
ud = [ui.diff(t) for ui in u] # Generalized speeds time derivatives
ua = dynamicsymbols('ua:3') # Auxiliary generalized speeds
CF = dynamicsymbols('Rx Ry Rz') # Contact forces
r = dynamicsymbols('r:3') # Coordinates, in R frame, from O to P
rd = [ri.diff(t) for ri in r] # Time derivatives of xi
N = ReferenceFrame('N') # Inertial Reference Frame
Y = N.orientnew('Y', 'Axis', [q[0], N.z]) # Yaw Frame
L = Y.orientnew('L', 'Axis', [q[1], Y.x]) # Lean Frame
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
def __init__(self):
self.num_links = 5 # Number of links
total_link_length = 1.
total_link_mass = 1.
self.ind_link_length = total_link_length / self.num_links
ind_link_com_length = self.ind_link_length / 2.
ind_link_mass = total_link_mass / self.num_links
ind_link_inertia = ind_link_mass * (ind_link_com_length**2)
# =======================#
# Parameters for step() #
# =======================#
# Maximum number of steps before episode termination
self.max_steps = 200
# For ODE integration
self.dt = .0001 # Simultaion time step = 1ms
self.sim_steps = 51 # Number of simulation steps in 1 learning step
self.dt_step = np.linspace(
0., self.dt * self.sim_steps,
num=self.sim_steps) # Learning time step = 50ms
# Termination conditions for simulation
self.num_steps = 0 # Step counter
self.done = False
# For visualisation
self.viewer = None
self.ax = False
# Constraints for observation
min_angle = -np.pi # Angle
max_angle = np.pi
min_omega = -10. # Angular velocity
max_omega = 10.
min_torque = -10. # Torque
max_torque = 10.
low_state_angle = np.full(self.num_links, min_angle) # Min angle
low_state_omega = np.full(self.num_links,
min_omega) # Min angular velocity
low_state = np.append(low_state_angle, low_state_omega)
high_state_angle = np.full(self.num_links, max_angle) # Max angle
high_state_omega = np.full(self.num_links,
max_omega) # Max angular velocity
high_state = np.append(high_state_angle, high_state_omega)
low_action = np.full(self.num_links, min_torque) # Min torque
high_action = np.full(self.num_links, max_torque) # Max torque
self.action_space = spaces.Box(low=low_action, high=high_action)
self.observation_space = spaces.Box(low=low_state, high=high_state)
# Minimum reward
self.min_reward = -(max_angle**2 + .1 * max_omega**2 +
.001 * max_torque**2) * self.num_links
# Seeding
self.seed()
# ==============#
# Orientations #
# ==============#
self.inertial_frame = ReferenceFrame('I')
self.link_frame = []
self.theta = []
for i in range(self.num_links):
temp_angle_name = "theta{}".format(i + 1)
temp_link_name = "L{}".format(i + 1)
self.theta.append(dynamicsymbols(temp_angle_name))
self.link_frame.append(ReferenceFrame(temp_link_name))
if i == 0: # First link
self.link_frame[i].orient(
self.inertial_frame, 'Axis',
(self.theta[i], self.inertial_frame.z))
else: # Second link, third link...
self.link_frame[i].orient(
self.link_frame[i - 1], 'Axis',
(self.theta[i], self.link_frame[i - 1].z))
# =================#
# Point Locations #
# =================#
# --------#
# Joints #
# --------#
self.link_length = []
self.link_joint = []
for i in range(self.num_links):
temp_link_length_name = "l_L{}".format(i + 1)
temp_link_joint_name = "A{}".format(i)
self.link_length.append(symbols(temp_link_length_name))
self.link_joint.append(Point(temp_link_joint_name))
if i > 0: # Set position started from link2, then link3, link4...
self.link_joint[i].set_pos(
self.link_joint[i - 1],
self.link_length[i - 1] * self.link_frame[i - 1].y)
# --------------------------#
# Centre of mass locations #
# --------------------------#
self.link_com_length = []
self.link_mass_centre = []
for i in range(self.num_links):
temp_link_com_length_name = "d_L{}".format(i + 1)
temp_link_mass_centre_name = "L{}_o".format(i + 1)
self.link_com_length.append(symbols(temp_link_com_length_name))
self.link_mass_centre.append(Point(temp_link_mass_centre_name))
self.link_mass_centre[i].set_pos(
self.link_joint[i],
self.link_com_length[i] * self.link_frame[i].y)
# ===========================================#
# Define kinematical differential equations #
# ===========================================#
self.omega = []
self.kinematical_differential_equations = []
self.time = symbols('t')
for i in range(self.num_links):
temp_omega_name = "omega{}".format(i + 1)
self.omega.append(dynamicsymbols(temp_omega_name))
self.kinematical_differential_equations.append(
self.omega[i] - self.theta[i].diff(self.time))
# ====================#
# Angular Velocities #
# ====================#
for i in range(self.num_links):
if i == 0: # First link
self.link_frame[i].set_ang_vel(
self.inertial_frame, self.omega[i] * self.inertial_frame.z)
else: # Second link, third link...
self.link_frame[i].set_ang_vel(
self.link_frame[i - 1],
self.omega[i] * self.link_frame[i - 1].z)
# ===================#
# Linear Velocities #
# ===================#
for i in range(self.num_links):
if i == 0: # First link
self.link_joint[i].set_vel(self.inertial_frame, 0)
else: # Second link, third link...
self.link_joint[i].v2pt_theory(self.link_joint[i - 1],
self.inertial_frame,
self.link_frame[i - 1])
self.link_mass_centre[i].v2pt_theory(self.link_joint[i],
self.inertial_frame,
self.link_frame[i])
# ======#
# Mass #
# ======#
self.link_mass = []
for i in range(self.num_links):
temp_link_mass_name = "m_L{}".format(i + 1)
self.link_mass.append(symbols(temp_link_mass_name))
# =========#
# Inertia #
# =========#
self.link_inertia = []
self.link_inertia_dyadic = []
self.link_central_inertia = []
for i in range(self.num_links):
temp_link_inertia_name = "I_L{}z".format(i + 1)
self.link_inertia.append(symbols(temp_link_inertia_name))
self.link_inertia_dyadic.append(
inertia(self.link_frame[i], 0, 0, self.link_inertia[i]))
self.link_central_inertia.append(
(self.link_inertia_dyadic[i], self.link_mass_centre[i]))
# ==============#
# Rigid Bodies #
# ==============#
self.link = []
for i in range(self.num_links):
temp_link_name = "link{}".format(i + 1)
self.link.append(
RigidBody(temp_link_name, self.link_mass_centre[i],
self.link_frame[i], self.link_mass[i],
self.link_central_inertia[i]))
# =========#
# Gravity #
# =========#
self.g = symbols('g')
self.link_grav_force = []
for i in range(self.num_links):
self.link_grav_force.append(
(self.link_mass_centre[i],
-self.link_mass[i] * self.g * self.inertial_frame.y))
# ===============#
# Joint Torques #
# ===============#
self.link_joint_torque = []
self.link_torque = []
for i in range(self.num_links):
temp_link_joint_torque_name = "T_a{}".format(i + 1)
self.link_joint_torque.append(
dynamicsymbols(temp_link_joint_torque_name))
for i in range(self.num_links):
if (i + 1) == self.num_links: # Last link
self.link_torque.append(
(self.link_frame[i],
self.link_joint_torque[i] * self.inertial_frame.z))
else: # Other links
self.link_torque.append(
(self.link_frame[i],
self.link_joint_torque[i] * self.inertial_frame.z -
self.link_joint_torque[i + 1] * self.inertial_frame.z))
# =====================#
# Equations of Motion #
# =====================#
self.coordinates = []
self.speeds = []
self.loads = []
self.bodies = []
for i in range(self.num_links):
self.coordinates.append(self.theta[i])
self.speeds.append(self.omega[i])
self.loads.append(self.link_grav_force[i])
self.loads.append(self.link_torque[i])
self.bodies.append(self.link[i])
self.kane = KanesMethod(self.inertial_frame, self.coordinates,
self.speeds,
self.kinematical_differential_equations)
self.fr, self.frstar = self.kane.kanes_equations(
self.bodies, self.loads)
self.mass_matrix = self.kane.mass_matrix_full
self.forcing_vector = self.kane.forcing_full
# =============================#
# List the symbolic arguments #
# =============================#
# -----------#
# Constants #
# -----------#
self.constants = []
for i in range(self.num_links):
if (i + 1) != self.num_links:
self.constants.append(self.link_length[i])
self.constants.append(self.link_com_length[i])
self.constants.append(self.link_mass[i])
self.constants.append(self.link_inertia[i])
self.constants.append(self.g)
# --------------#
# Time Varying #
# --------------#
self.coordinates = []
self.speeds = []
self.specified = []
for i in range(self.num_links):
self.coordinates.append(self.theta[i])
self.speeds.append(self.omega[i])
self.specified.append(self.link_joint_torque[i])
# =======================#
# Generate RHS Function #
# =======================#
self.right_hand_side = generate_ode_function(
self.forcing_vector,
self.coordinates,
self.speeds,
self.constants,
mass_matrix=self.mass_matrix,
specifieds=self.specified)
# ==============================#
# Specify Numerical Quantities #
# ==============================#
self.x = np.zeros(self.num_links * 2)
self.x[:self.num_links] = deg2rad(2.0)
self.numerical_constants = []
for i in range(self.num_links):
if (i + 1) != self.num_links:
self.numerical_constants.append(self.ind_link_length)
self.numerical_constants.append(ind_link_com_length)
self.numerical_constants.append(ind_link_mass)
self.numerical_constants.append(ind_link_inertia)
self.numerical_constants.append(9.81)
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])
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'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)
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(q2), 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
# 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 = 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()
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}
# 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
# Compute the integral by integrating over the whole volume of the tours
I_ab = integrate(
integrate(integrate(integrand, (phi, 0, 2 * pi)), (theta, 0, 2 * pi)),
(s, 0, r))
inertia_scalars.append(I_ab)
# Create an inertia dyad from the list of inertia scalars
I_A_O = inertia(A, *inertia_scalars)
print("Inertia of torus about mass center = {0}".format(I_A_O))
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)))
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
# add the term to get u3 from u1 to kde_map
kde_map[dot(pC_star.vel(N), N.z)] = u3
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
# central inertias, rigid bodies
IA = inertia(A, A1, mA*kA**2, A3)
IB = inertia(B, B1, mB*kB**2, B3)
IC = inertia(C, C1, C2, C3)
# inertia is defined as (central inertia, mass center) 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))
bodies = [rbA, rbB, rbC]
# forces, torques
forces = [(pO, P1*N.x + P2*N.y + P3*N.z),
(pP, R1*N.x + R2*N.y + R3*N.z),
(pP_prime, -(R1*N.x + R2*N.y + R3*N.z)),
(pC_star, Q1*N.x + Q2*N.y + Q3*N.z)]
torques = [(A, alpha1*N.x + alpha2*N.y + alpha3*N.z),
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()))
v.set_acc(N, av1 * B['1'] + av2 * B['2'] + av3 * B['3']) #------------------------------------------------------------# # calculate the acceleration of the handlebar center of mass # #------------------------------------------------------------# s.a2pt_theory(v, N, B) go.a2pt_theory(s, N, G) #-------------------------------# # handlebar equations of motion # #-------------------------------# # calculate the angular momentum of the handlebar in N about the center of mass # of the handlebar IG = me.inertia(G, IG11, IG22, IG33, 0, 0, IG31) H_G_N_go = IG.dot(G.ang_vel_in(N)) Hdot = H_G_N_go.dt(N) # euler's equation about an arbitrary point, s sumT = Hdot + go.pos_from(s).cross(mG * go.acc(N)) # calculate the steer torque Tdelta = sumT.dot(G['3']) + Tm + Tu # let's make use of the steer rate gyro and frame rate gyro measurement instead # of differentiating delta Tdelta = Tdelta.subs(deltad, wg3 - wb3) print "Tdelta as a function of the measured data:\nTdelta =", Tdelta
body_mass_center.v2pt_theory(l_hip, inertial_frame, body_frame)
r_leg_mass_center.v2pt_theory(r_hip, inertial_frame, body_frame)
# Mass
# ====
l_leg_mass, body_mass, r_leg_mass = symbols("m_L, m_B, m_R")
# Inertia
# =======
l_leg_inertia, body_inertia, r_leg_inertia = symbols("I_Lz, I_Bz, I_Rz")
l_leg_inertia_dyadic = inertia(l_leg_frame, 0, 0, l_leg_inertia)
l_leg_central_inertia = (l_leg_inertia_dyadic, l_leg_mass_center)
body_inertia_dyadic = inertia(body_frame, 0, 0, body_inertia)
body_central_inertia = (body_inertia_dyadic, body_mass_center)
r_leg_inertia_dyadic = inertia(body_frame, 0, 0, r_leg_inertia)
r_leg_central_inertia = (r_leg_inertia_dyadic, r_leg_mass_center)
# Rigid Bodies
# ============
l_leg = RigidBody("Lower Leg", l_leg_mass_center, l_leg_frame, l_leg_mass, l_leg_central_inertia)
v.set_acc(N, av1 * B['1'] + av2 * B['2'] + av3 * B['3'])
#------------------------------------------------------------#
# calculate the acceleration of the handlebar center of mass #
#------------------------------------------------------------#
s.a2pt_theory(v, N, B)
go.a2pt_theory(s, N, G)
#-------------------------------#
# handlebar equations of motion #
#-------------------------------#
# calculate the angular momentum of the handlebar in N about the center of mass
# of the handlebar
IG = me.inertia(G, IG11, IG22, IG33, 0, 0, IG31)
H_G_N_go = IG.dot(G.ang_vel_in(N))
Hdot = H_G_N_go.dt(N)
# euler's equation about an arbitrary point, s
sumT = Hdot + go.pos_from(s).cross(mG * go.acc(N))
# calculate the steer torque
Tdelta = sumT.dot(G['3']) + Tm + Tu
# let's make use of the steer rate gyro and frame rate gyro measurement instead
# of differentiating delta
Tdelta = Tdelta.subs(deltad, wg3 - wb3)
msg = "Tdelta as a function of the measured data:\nTdelta = {}"
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}
# 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
forces = [(pC_hat, R_C_hat), (pC_star, R_C_star)]
# partial velocities
bodies = [rbC]
system = [i.masscenter for i in bodies] + [i.frame for i in bodies] + list(zip(*forces)[0])
partials = partial_velocities(system, [u1, u2, u3], A, kde_map, vc_map)
# generalized active forces
Fr, _ = generalized_active_forces(partials, forces)
Fr_star, _ = generalized_inertia_forces(partials, bodies, kde_map, vc_map)
body_a_f.set_ang_vel(frame_n, omega*frame_n.y)
body_b_f.set_ang_vel(body_a_f, alpha*body_a_f.z)
point_o.set_vel(frame_n, 0)
body_a_cm.v2pt_theory(point_o,frame_n,body_a_f)
body_b_cm.v2pt_theory(point_o,frame_n,body_a_f)
ma = sm.symbols('ma')
body_a.mass = ma
mb = sm.symbols('mb')
body_b.mass = mb
iaxx = 1/12*ma*(2*la)**2
iayy = iaxx
iazz = 0
ibxx = 1/12*mb*h**2
ibyy = 1/12*mb*(w**2+h**2)
ibzz = 1/12*mb*w**2
body_a.inertia = (me.inertia(body_a_f, iaxx, iayy, iazz, 0, 0, 0), body_a_cm)
body_b.inertia = (me.inertia(body_b_f, ibxx, ibyy, ibzz, 0, 0, 0), body_b_cm)
force_a = body_a.mass*(g*frame_n.z)
force_b = body_b.mass*(g*frame_n.z)
kd_eqs = [thetad - omega, phid - alpha]
forceList = [(body_a.masscenter,body_a.mass*(g*frame_n.z)), (body_b.masscenter,body_b.mass*(g*frame_n.z))]
kane = me.KanesMethod(frame_n, q_ind=[theta,phi], u_ind=[omega, alpha], kd_eqs = kd_eqs)
fr, frstar = kane.kanes_equations([body_a, body_b], forceList)
zero = fr+frstar
from pydy.system import System
sys = System(kane, constants = {g:9.81, lb:0.2, w:0.2, h:0.1, ma:0.01, mb:0.1},
specifieds={},
initial_conditions={theta:np.deg2rad(90), phi:np.deg2rad(0.5), omega:0, alpha:0},
times = np.linspace(0.0, 10, 10/0.02))
y=sys.integrate()
CMa.locatenew('AB_3', L[33] * A.x + L[34] * A.y - L[35] * A.z)
]
B = A.orientnew('B', 'Body', (q[6], q[7], q[8]), '123')
B.set_ang_vel(A, u[6] * A.x + u[7] * A.y + u[8] * A.z)
CMb = CMa.locatenew('CM_b', q[9] * A.x + q[10] * A.y + q[11] * A.z)
CMb.set_vel(A, u[9] * A.x + u[10] * A.y + u[11] * A.z)
CMb.set_vel(N, CMb.vel(A).express(N))
BA = [
CMb.locatenew('BA_0', L[36] * B.x - L[37] * B.y + L[38] * B.z),
CMb.locatenew('BA_1', -L[39] * B.x - L[40] * B.y + L[41] * B.z),
CMb.locatenew('BA_2', -L[42] * B.x - L[43] * B.y - L[44] * B.z),
CMb.locatenew('BA_3', L[45] * B.x - L[46] * B.y - L[47] * B.z)
]
In = [me.inertia(A, I[0], I[1], I[2]), me.inertia(B, I[3], I[4], I[5])]
bodies = [
me.RigidBody('Shell', CMa, A, ma, (In[0], CMa)),
me.RigidBody('Block', CMb, B, mb, (In[1], CMb))
]
loads = [(CMa, -ma * g * N.y), (CMb, -mb * g * N.y)]
# CMa.set_vel(N, CMa.pos_from(Orig).dt(N))
# CMb.set_vel(N, CMb.pos_from(Orig).dt(N))
# CMb.set_vel(A, CMb.pos_from(CMa).dt(A))
for _i in range(4):
OA[_i].v2pt_theory(Orig, N, N)
AO[_i].v2pt_theory(CMa, N, A)
AB[_i].v2pt_theory(CMa, N, A)
BA[_i].v2pt_theory(CMb, N, B)
v_SAF_1 = do.vel(N) + mec.cross(C.ang_vel_in(N), SAF.pos_from(do))
v_SAF_2 = fo.vel(N) + mec.cross(E.ang_vel_in(N), SAF.pos_from(fo))
print("ready for constraints; inertia; rigid bodies; bodylist")
####################
# Motion Constraints
####################
holonomic = [fn.pos_from(dn).dot(A["3"])]
nonholonomic = [(v_SAF_1 - v_SAF_2).dot(uv) for uv in E]
#########
# Inertia
#########
Ic = mec.inertia(C, ic11, ic22, ic33, 0.0, 0.0, ic31)
Id = mec.inertia(C, id11, id22, id11, 0.0, 0.0, 0.0) # rear wheel
Ie = mec.inertia(E, ie11, ie22, ie33, 0.0, 0.0, ie31)
If = mec.inertia(E, if11, if22, if11, 0.0, 0.0, 0.0) # front wheel
##############
# Rigid Bodies
##############
rearFrame_inertia = (Ic, co)
rearFrame = mec.RigidBody("rearFrame", co, C, mc, rearFrame_inertia)
rearWheel_inertia = (Id, do)
rearWheel = mec.RigidBody("rearWheel", do, D, md, rearWheel_inertia)
frontFrame_inertia = (Ie, eo)
# 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)))))
