def test_center_of_mass():
a = ReferenceFrame('a')
m = symbols('m', real=True)
p1 = Particle('p1', Point('p1_pt'), S.One)
p2 = Particle('p2', Point('p2_pt'), S(2))
p3 = Particle('p3', Point('p3_pt'), S(3))
p4 = Particle('p4', Point('p4_pt'), m)
b_f = ReferenceFrame('b_f')
b_cm = Point('b_cm')
mb = symbols('mb')
b = RigidBody('b', b_cm, b_f, mb, (outer(b_f.x, b_f.x), b_cm))
p2.point.set_pos(p1.point, a.x)
p3.point.set_pos(p1.point, a.x + a.y)
p4.point.set_pos(p1.point, a.y)
b.masscenter.set_pos(p1.point, a.y + a.z)
point_o=Point('o')
point_o.set_pos(p1.point, center_of_mass(p1.point, p1, p2, p3, p4, b))
expr = 5/(m + mb + 6)*a.x + (m + mb + 3)/(m + mb + 6)*a.y + mb/(m + mb + 6)*a.z
assert point_o.pos_from(p1.point)-expr == 0
def kinetic_energy(frame, *body):
"""Kinetic energy of a multibody system.
This function returns the kinetic energy of a system of Particle's and/or
RigidBody's. The kinetic energy of such a system is equal to the sum of
the kinetic energies of its constituents. Consider a system, S, comprising
a rigid body, A, and a particle, P. The kinetic energy of the system, T,
is equal to the vector sum of the kinetic energy of the particle, T1, and
the kinetic energy of the rigid body, T2, i.e.
T = T1 + T2
Kinetic energy is a scalar.
Parameters
==========
frame : ReferenceFrame
The frame in which the velocity or angular velocity of the body is
defined.
body1, body2, body3... : Particle and/or RigidBody
The body (or bodies) whose kinetic energy is required.
Examples
========
>>> from sympy.physics.mechanics import Point, Particle, ReferenceFrame
>>> from sympy.physics.mechanics import RigidBody, outer, kinetic_energy
>>> N = ReferenceFrame('N')
>>> O = Point('O')
>>> O.set_vel(N, 0 * N.x)
>>> P = O.locatenew('P', 1 * N.x)
>>> P.set_vel(N, 10 * N.x)
>>> Pa = Particle('Pa', P, 1)
>>> Ac = O.locatenew('Ac', 2 * N.y)
>>> Ac.set_vel(N, 5 * N.y)
>>> a = ReferenceFrame('a')
>>> a.set_ang_vel(N, 10 * N.z)
>>> I = outer(N.z, N.z)
>>> A = RigidBody('A', Ac, a, 20, (I, Ac))
>>> kinetic_energy(N, Pa, A)
350
"""
if not isinstance(frame, ReferenceFrame):
raise TypeError('Please enter a valid ReferenceFrame')
ke_sys = S(0)
for e in body:
if isinstance(e, (RigidBody, Particle)):
ke_sys += e.kinetic_energy(frame)
else:
raise TypeError('*body must have only Particle or RigidBody')
return ke_sys
def test_center_of_mass():
a = ReferenceFrame("a")
m = symbols("m", real=True)
p1 = Particle("p1", Point("p1_pt"), S.One)
p2 = Particle("p2", Point("p2_pt"), S(2))
p3 = Particle("p3", Point("p3_pt"), S(3))
p4 = Particle("p4", Point("p4_pt"), m)
b_f = ReferenceFrame("b_f")
b_cm = Point("b_cm")
mb = symbols("mb")
b = RigidBody("b", b_cm, b_f, mb, (outer(b_f.x, b_f.x), b_cm))
p2.point.set_pos(p1.point, a.x)
p3.point.set_pos(p1.point, a.x + a.y)
p4.point.set_pos(p1.point, a.y)
b.masscenter.set_pos(p1.point, a.y + a.z)
point_o = Point("o")
point_o.set_pos(p1.point, center_of_mass(p1.point, p1, p2, p3, p4, b))
expr = (5 / (m + mb + 6) * a.x + (m + mb + 3) / (m + mb + 6) * a.y + mb /
(m + mb + 6) * a.z)
assert point_o.pos_from(p1.point) - expr == 0
def potential_energy(*body):
"""Potential energy of a multibody system.
This function returns the potential energy of a system of Particle's and/or
RigidBody's. The potential energy of such a system is equal to the sum of
the potential energy of its constituents. Consider a system, S, comprising
a rigid body, A, and a particle, P. The potential energy of the system, V,
is equal to the vector sum of the potential energy of the particle, V1, and
the potential energy of the rigid body, V2, i.e.
V = V1 + V2
Potential energy is a scalar.
Parameters
==========
body1, body2, body3... : Particle and/or RigidBody
The body (or bodies) whose potential energy is required.
Examples
========
>>> from sympy.physics.mechanics import Point, Particle, ReferenceFrame
>>> from sympy.physics.mechanics import RigidBody, outer, potential_energy
>>> from sympy import symbols
>>> M, m, g, h = symbols('M m g h')
>>> N = ReferenceFrame('N')
>>> O = Point('O')
>>> O.set_vel(N, 0 * N.x)
>>> P = O.locatenew('P', 1 * N.x)
>>> Pa = Particle('Pa', P, m)
>>> Ac = O.locatenew('Ac', 2 * N.y)
>>> a = ReferenceFrame('a')
>>> I = outer(N.z, N.z)
>>> A = RigidBody('A', Ac, a, M, (I, Ac))
>>> Pa.potential_energy = m * g * h
>>> A.potential_energy = M * g * h
>>> potential_energy(Pa, A)
M*g*h + g*h*m
"""
pe_sys = S(0)
for e in body:
if isinstance(e, (RigidBody, Particle)):
pe_sys += e.potential_energy
else:
raise TypeError('*body must have only Particle or RigidBody')
return pe_sys
def __and__(self, other):
"""Dot product of two vectors.
Returns a scalar, the dot product of the two Vectors
Parameters
==========
other : Vector
The Vector which we are dotting with
Examples
========
>>> from sympy.physics.vector import ReferenceFrame, dot
>>> from sympy import symbols
>>> q1 = symbols('q1')
>>> N = ReferenceFrame('N')
>>> dot(N.x, N.x)
1
>>> dot(N.x, N.y)
0
>>> A = N.orientnew('A', 'Axis', [q1, N.x])
>>> dot(N.y, A.y)
cos(q1)
"""
from sympy.physics.vector.dyadic import Dyadic
if isinstance(other, Dyadic):
return NotImplemented
other = _check_vector(other)
out = S(0)
for i, v1 in enumerate(self.args):
for j, v2 in enumerate(other.args):
out += ((v2[0].T)
* (v2[1].dcm(v1[1]))
* (v1[0]))[0]
if Vector.simp:
return trigsimp(sympify(out), recursive=True)
else:
return sympify(out)
def get_motion_params(frame, **kwargs):
"""
Returns the three motion parameters - (acceleration, velocity, and
position) as vectorial functions of time in the given frame.
If a higher order differential function is provided, the lower order
functions are used as boundary conditions. For example, given the
acceleration, the velocity and position parameters are taken as
boundary conditions.
The values of time at which the boundary conditions are specified
are taken from timevalue1(for position boundary condition) and
timevalue2(for velocity boundary condition).
If any of the boundary conditions are not provided, they are taken
to be zero by default (zero vectors, in case of vectorial inputs). If
the boundary conditions are also functions of time, they are converted
to constants by substituting the time values in the dynamicsymbols._t
time Symbol.
This function can also be used for calculating rotational motion
parameters. Have a look at the Parameters and Examples for more clarity.
Parameters
==========
frame : ReferenceFrame
The frame to express the motion parameters in
acceleration : Vector
Acceleration of the object/frame as a function of time
velocity : Vector
Velocity as function of time or as boundary condition
of velocity at time = timevalue1
position : Vector
Velocity as function of time or as boundary condition
of velocity at time = timevalue1
timevalue1 : sympyfiable
Value of time for position boundary condition
timevalue2 : sympyfiable
Value of time for velocity boundary condition
Examples
========
>>> from sympy.physics.vector import ReferenceFrame, get_motion_params, dynamicsymbols
>>> from sympy import symbols
>>> R = ReferenceFrame('R')
>>> v1, v2, v3 = dynamicsymbols('v1 v2 v3')
>>> v = v1*R.x + v2*R.y + v3*R.z
>>> get_motion_params(R, position = v)
(v1''*R.x + v2''*R.y + v3''*R.z, v1'*R.x + v2'*R.y + v3'*R.z, v1*R.x + v2*R.y + v3*R.z)
>>> a, b, c = symbols('a b c')
>>> v = a*R.x + b*R.y + c*R.z
>>> get_motion_params(R, velocity = v)
(0, a*R.x + b*R.y + c*R.z, a*t*R.x + b*t*R.y + c*t*R.z)
>>> parameters = get_motion_params(R, acceleration = v)
>>> parameters[1]
a*t*R.x + b*t*R.y + c*t*R.z
>>> parameters[2]
a*t**2/2*R.x + b*t**2/2*R.y + c*t**2/2*R.z
"""
##Helper functions
def _process_vector_differential(vectdiff, condition, \
variable, ordinate, frame):
"""
Helper function for get_motion methods. Finds derivative of vectdiff wrt
variable, and its integral using the specified boundary condition at
value of variable = ordinate.
Returns a tuple of - (derivative, function and integral) wrt vectdiff
"""
#Make sure boundary condition is independent of 'variable'
if condition != 0:
condition = express(condition, frame, variables=True)
#Special case of vectdiff == 0
if vectdiff == Vector(0):
return (0, 0, condition)
#Express vectdiff completely in condition's frame to give vectdiff1
vectdiff1 = express(vectdiff, frame)
#Find derivative of vectdiff
vectdiff2 = time_derivative(vectdiff, frame)
#Integrate and use boundary condition
vectdiff0 = Vector(0)
lims = (variable, ordinate, variable)
for dim in frame:
function1 = vectdiff1.dot(dim)
abscissa = dim.dot(condition).subs({variable: ordinate})
# Indefinite integral of 'function1' wrt 'variable', using
# the given initial condition (ordinate, abscissa).
vectdiff0 += (integrate(function1, lims) + abscissa) * dim
#Return tuple
return (vectdiff2, vectdiff, vectdiff0)
##Function body
_check_frame(frame)
#Decide mode of operation based on user's input
if 'acceleration' in kwargs:
mode = 2
elif 'velocity' in kwargs:
mode = 1
else:
mode = 0
#All the possible parameters in kwargs
#Not all are required for every case
#If not specified, set to default values(may or may not be used in
#calculations)
conditions = [
'acceleration', 'velocity', 'position', 'timevalue', 'timevalue1',
'timevalue2'
]
for i, x in enumerate(conditions):
if x not in kwargs:
if i < 3:
kwargs[x] = Vector(0)
else:
kwargs[x] = S(0)
elif i < 3:
_check_vector(kwargs[x])
else:
kwargs[x] = sympify(kwargs[x])
if mode == 2:
vel = _process_vector_differential(kwargs['acceleration'],
kwargs['velocity'],
dynamicsymbols._t,
kwargs['timevalue2'], frame)[2]
pos = _process_vector_differential(vel, kwargs['position'],
dynamicsymbols._t,
kwargs['timevalue1'], frame)[2]
return (kwargs['acceleration'], vel, pos)
elif mode == 1:
return _process_vector_differential(kwargs['velocity'],
kwargs['position'],
dynamicsymbols._t,
kwargs['timevalue1'], frame)
else:
vel = time_derivative(kwargs['position'], frame)
acc = time_derivative(vel, frame)
return (acc, vel, kwargs['position'])
def test_type_F():
c = CartanType("F4")
m = Matrix(4, 4, [2, -1, 0, 0, -1, 2, -2, 0, 0, -1, 2, -1, 0, 0, -1, 2])
assert c.cartan_matrix() == m
assert c.dimension() == 4
assert c.simple_root(3) == [0, 0, 0, 1]
assert c.simple_root(4) == [-S(1) / 2, -S(1) / 2, -S(1) / 2, -S(1) / 2]
assert c.roots() == 48
assert c.basis() == 52
diag = "0---0=>=0---0\n" + " ".join(str(i) for i in range(1, 5))
assert c.dynkin_diagram() == diag
assert c.positive_roots() == {
1: [1, -1, 0, 0],
2: [1, 1, 0, 0],
3: [1, 0, -1, 0],
4: [1, 0, 1, 0],
5: [1, 0, 0, -1],
6: [1, 0, 0, 1],
7: [0, 1, -1, 0],
8: [0, 1, 1, 0],
9: [0, 1, 0, -1],
10: [0, 1, 0, 1],
11: [0, 0, 1, -1],
12: [0, 0, 1, 1],
13: [1, 0, 0, 0],
14: [0, 1, 0, 0],
15: [0, 0, 1, 0],
16: [0, 0, 0, 1],
17: [S(1) / 2, S(1) / 2, S(1) / 2,
S(1) / 2],
18: [S(1) / 2, S(-1) / 2, S(1) / 2,
S(1) / 2],
19: [S(1) / 2, S(1) / 2, S(-1) / 2,
S(1) / 2],
20: [S(1) / 2, S(1) / 2, S(1) / 2,
S(-1) / 2],
21: [S(1) / 2, S(1) / 2, S(-1) / 2,
S(-1) / 2],
22: [S(1) / 2, S(-1) / 2, S(1) / 2,
S(-1) / 2],
23: [S(1) / 2, S(-1) / 2, S(-1) / 2,
S(1) / 2],
24: [S(1) / 2, S(-1) / 2, S(-1) / 2,
S(-1) / 2]
}
def test_linearize_rolling_disc_kane():
# Symbols for time and constant parameters
t, r, m, g, v = symbols('t r m g v')
# Configuration variables and their time derivatives
q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7')
q1d, q2d, q3d, q4d, q5d, q6d = qd = [qi.diff(t) for qi in q]
# Generalized speeds and their time derivatives
u = dynamicsymbols('u:6')
u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7')
u1d, u2d, u3d, u4d, u5d, u6d = [ui.diff(t) for ui in u]
# Reference frames
N = ReferenceFrame('N') # Inertial frame
NO = Point('NO') # Inertial origin
A = N.orientnew('A', 'Axis', [q1, N.z]) # Yaw intermediate frame
B = A.orientnew('B', 'Axis', [q2, A.x]) # Lean intermediate frame
C = B.orientnew('C', 'Axis', [q3, B.y]) # Disc fixed frame
CO = NO.locatenew('CO', q4 * N.x + q5 * N.y + q6 * N.z) # Disc center
# Disc angular velocity in N expressed using time derivatives of coordinates
w_c_n_qd = C.ang_vel_in(N)
w_b_n_qd = B.ang_vel_in(N)
# Inertial angular velocity and angular acceleration of disc fixed frame
C.set_ang_vel(N, u1 * B.x + u2 * B.y + u3 * B.z)
# Disc center velocity in N expressed using time derivatives of coordinates
v_co_n_qd = CO.pos_from(NO).dt(N)
# Disc center velocity in N expressed using generalized speeds
CO.set_vel(N, u4 * C.x + u5 * C.y + u6 * C.z)
# Disc Ground Contact Point
P = CO.locatenew('P', r * B.z)
P.v2pt_theory(CO, N, C)
# Configuration constraint
f_c = Matrix([q6 - dot(CO.pos_from(P), N.z)])
# Velocity level constraints
f_v = Matrix([dot(P.vel(N), uv) for uv in C])
# Kinematic differential equations
kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
[dot(v_co_n_qd - CO.vel(N), uv) for uv in N])
qdots = solve(kindiffs, qd)
# Set angular velocity of remaining frames
B.set_ang_vel(N, w_b_n_qd.subs(qdots))
C.set_ang_acc(
N,
C.ang_vel_in(N).dt(B) + cross(B.ang_vel_in(N), C.ang_vel_in(N)))
# Active forces
F_CO = m * g * A.z
# Create inertia dyadic of disc C about point CO
I = (m * r**2) / 4
J = (m * r**2) / 2
I_C_CO = inertia(C, I, J, I)
Disc = RigidBody('Disc', CO, C, m, (I_C_CO, CO))
BL = [Disc]
FL = [(CO, F_CO)]
KM = KanesMethod(N, [q1, q2, q3, q4, q5], [u1, u2, u3],
kd_eqs=kindiffs,
q_dependent=[q6],
configuration_constraints=f_c,
u_dependent=[u4, u5, u6],
velocity_constraints=f_v)
with warns_deprecated_sympy():
(fr, fr_star) = KM.kanes_equations(FL, BL)
# Test generalized form equations
linearizer = KM.to_linearizer()
assert linearizer.f_c == f_c
assert linearizer.f_v == f_v
assert linearizer.f_a == f_v.diff(t)
sol = solve(linearizer.f_0 + linearizer.f_1, qd)
for qi in qd:
assert sol[qi] == qdots[qi]
assert simplify(linearizer.f_2 + linearizer.f_3 - fr - fr_star) == Matrix(
[0, 0, 0])
# Perform the linearization
# Precomputed operating point
q_op = {q6: -r * cos(q2)}
u_op = {
u1: 0,
u2: sin(q2) * q1d + q3d,
u3: cos(q2) * q1d,
u4: -r * (sin(q2) * q1d + q3d) * cos(q3),
u5: 0,
u6: -r * (sin(q2) * q1d + q3d) * sin(q3)
}
qd_op = {
q2d: 0,
q4d: -r * (sin(q2) * q1d + q3d) * cos(q1),
q5d: -r * (sin(q2) * q1d + q3d) * sin(q1),
q6d: 0
}
ud_op = {
u1d:
4 * g * sin(q2) / (5 * r) + sin(2 * q2) * q1d**2 / 2 +
6 * cos(q2) * q1d * q3d / 5,
u2d:
0,
u3d:
0,
u4d:
r * (sin(q2) * sin(q3) * q1d * q3d + sin(q3) * q3d**2),
u5d:
r * (4 * g * sin(q2) /
(5 * r) + sin(2 * q2) * q1d**2 / 2 + 6 * cos(q2) * q1d * q3d / 5),
u6d:
-r * (sin(q2) * cos(q3) * q1d * q3d + cos(q3) * q3d**2)
}
A, B = linearizer.linearize(op_point=[q_op, u_op, qd_op, ud_op],
A_and_B=True,
simplify=True)
upright_nominal = {q1d: 0, q2: 0, m: 1, r: 1, g: 1}
# Precomputed solution
A_sol = Matrix([[0, 0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0],
[sin(q1) * q3d, 0, 0, 0, 0, -sin(q1), -cos(q1), 0],
[-cos(q1) * q3d, 0, 0, 0, 0,
cos(q1), -sin(q1), 0],
[0, S(4) / 5, 0, 0, 0, 0, 0, 6 * q3d / 5],
[0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, -2 * q3d, 0, 0]])
B_sol = Matrix([])
# Check that linearization is correct
assert A.subs(upright_nominal) == A_sol
assert B.subs(upright_nominal) == B_sol
# Check eigenvalues at critical speed are all zero:
assert A.subs(upright_nominal).subs(q3d, 1 / sqrt(3)).eigenvals() == {0: 8}
def test_rolling_disc():
# Rolling Disc Example
# Here the rolling disc is formed from the contact point up, removing the
# need to introduce generalized speeds. Only 3 configuration and three
# speed variables are need to describe this system, along with the disc's
# mass and radius, and the local gravity (note that mass will drop out).
q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
r, m, g = symbols('r m g')
# The kinematics are formed by a series of simple rotations. Each simple
# rotation creates a new frame, and the next rotation is defined by the new
# frame's basis vectors. This example uses a 3-1-2 series of rotations, or
# Z, X, Y series of rotations. Angular velocity for this is defined using
# the second frame's basis (the lean frame).
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Axis', [q1, N.z])
L = Y.orientnew('L', 'Axis', [q2, Y.x])
R = L.orientnew('R', 'Axis', [q3, L.y])
w_R_N_qd = R.ang_vel_in(N)
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
# This is the translational kinematics. We create a point with no velocity
# in N; this is the contact point between the disc and ground. Next we form
# the position vector from the contact point to the disc's center of mass.
# Finally we form the velocity and acceleration of the disc.
C = Point('C')
C.set_vel(N, 0)
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
# This is a simple way to form the inertia dyadic.
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
# Kinematic differential equations; how the generalized coordinate time
# derivatives relate to generalized speeds.
kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]
# Creation of the force list; it is the gravitational force at the mass
# center of the disc. Then we create the disc by assigning a Point to the
# center of mass attribute, a ReferenceFrame to the frame attribute, and mass
# and inertia. Then we form the body list.
ForceList = [(Dmc, -m * g * Y.z)]
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
BodyList = [BodyD]
# Finally we form the equations of motion, using the same steps we did
# before. Specify inertial frame, supply generalized speeds, supply
# kinematic differential equation dictionary, compute Fr from the force
# list and Fr* from the body list, compute the mass matrix and forcing
# terms, then solve for the u dots (time derivatives of the generalized
# speeds).
KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
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(0): 6
}
