def test_trigsimp_groebner():
from sympy.simplify.trigsimp import trigsimp_groebner
c = cos(x)
s = sin(x)
ex = (4*s*c + 12*s + 5*c**3 + 21*c**2 + 23*c + 15)/(
-s*c**2 + 2*s*c + 15*s + 7*c**3 + 31*c**2 + 37*c + 21)
resnum = (5*s - 5*c + 1)
resdenom = (8*s - 6*c)
results = [resnum/resdenom, (-resnum)/(-resdenom)]
assert trigsimp_groebner(ex) in results
assert trigsimp_groebner(s/c, hints=[tan]) == tan(x)
assert trigsimp_groebner(c*s) == c*s
assert trigsimp((-s + 1)/c + c/(-s + 1),
method='groebner') == 2/c
assert trigsimp((-s + 1)/c + c/(-s + 1),
method='groebner', polynomial=True) == 2/c
# Test quick=False works
assert trigsimp_groebner(ex, hints=[2]) in results
# test "I"
assert trigsimp_groebner(sin(I*x)/cos(I*x), hints=[tanh]) == I*tanh(x)
# test hyperbolic / sums
assert trigsimp_groebner((tanh(x)+tanh(y))/(1+tanh(x)*tanh(y)),
hints=[(tanh, x, y)]) == tanh(x + y)
def _solve_type_2(symo, X, Y, Z, th):
"""Solution for the equation:
X*S + Y*C = Z
"""
symo.write_line("# X*sin({0}) + Y*cos({0}) = Z".format(th))
X = symo.replace(trigsimp(X), 'X', th)
Y = symo.replace(trigsimp(Y), 'Y', th)
Z = symo.replace(trigsimp(Z), 'Z', th)
YPS = var('YPS'+str(th))
if X == tools.ZERO and Y != tools.ZERO:
C = symo.replace(Z/Y, 'C', th)
symo.add_to_dict(YPS, (tools.ONE, - tools.ONE))
symo.add_to_dict(th, atan2(YPS*sqrt(1-C**2), C))
elif X != tools.ZERO and Y == tools.ZERO:
S = symo.replace(Z/X, 'S', th)
symo.add_to_dict(YPS, (tools.ONE, - tools.ONE))
symo.add_to_dict(th, atan2(S, YPS*sqrt(1-S**2)))
elif Z == tools.ZERO:
symo.add_to_dict(YPS, (tools.ONE, tools.ZERO))
symo.add_to_dict(th, atan2(-Y, X) + YPS*pi)
else:
B = symo.replace(X**2 + Y**2, 'B', th)
D = symo.replace(B - Z**2, 'D', th)
symo.add_to_dict(YPS, (tools.ONE, - tools.ONE))
S = symo.replace((X*Z + YPS * Y * sqrt(D))/B, 'S', th)
C = symo.replace((Y*Z - YPS * X * sqrt(D))/B, 'C', th)
symo.add_to_dict(th, atan2(S, C))
def test_quaternion_conversions():
q1 = Quaternion(1, 2, 3, 4)
assert q1.to_axis_angle() == ((2 * sqrt(29)/29,
3 * sqrt(29)/29,
4 * sqrt(29)/29),
2 * acos(sqrt(30)/30))
assert q1.to_rotation_matrix() == Matrix([[-S(2)/3, S(2)/15, S(11)/15],
[S(2)/3, -S(1)/3, S(14)/15],
[S(1)/3, S(14)/15, S(2)/15]])
assert q1.to_rotation_matrix((1, 1, 1)) == Matrix([[-S(2)/3, S(2)/15, S(11)/15, S(4)/5],
[S(2)/3, -S(1)/3, S(14)/15, -S(4)/15],
[S(1)/3, S(14)/15, S(2)/15, -S(2)/5],
[S(0), S(0), S(0), S(1)]])
theta = symbols("theta", real=True)
q2 = Quaternion(cos(theta/2), 0, 0, sin(theta/2))
assert trigsimp(q2.to_rotation_matrix()) == Matrix([
[cos(theta), -sin(theta), 0],
[sin(theta), cos(theta), 0],
[0, 0, 1]])
assert q2.to_axis_angle() == ((0, 0, sin(theta/2)/Abs(sin(theta/2))),
2*acos(cos(theta/2)))
assert trigsimp(q2.to_rotation_matrix((1, 1, 1))) == Matrix([
[cos(theta), -sin(theta), 0, sin(theta) - cos(theta) + 1],
[sin(theta), cos(theta), 0, -sin(theta) - cos(theta) + 1],
[0, 0, 1, 0],
[0, 0, 0, 1]])
def test_trigsimp2():
x, y = symbols('x,y')
assert trigsimp(cos(x)**2*sin(y)**2 + cos(x)**2*cos(y)**2 + sin(x)**2,
recursive=True) == 1
assert trigsimp(sin(x)**2*sin(y)**2 + sin(x)**2*cos(y)**2 + cos(x)**2,
recursive=True) == 1
assert trigsimp(
Subs(x, x, sin(y)**2 + cos(y)**2)) == Subs(x, x, 1)
def test_issue_15129_trigsimp_methods():
t1 = Matrix([sin(Rational(1, 50)), cos(Rational(1, 50)), 0])
t2 = Matrix([sin(Rational(1, 25)), cos(Rational(1, 25)), 0])
t3 = Matrix([cos(Rational(1, 25)), sin(Rational(1, 25)), 0])
r1 = t1.dot(t2)
r2 = t1.dot(t3)
assert trigsimp(r1) == cos(S(1)/50)
assert trigsimp(r2) == sin(S(3)/50)
def test_transformation(self):
theta = sympy.Symbol("theta")
trans = matrices.TTransX(theta)
trans_1 = matrices.TTransX(-theta)
self.assertEqual(sympy.trigsimp(trans.m * trans_1.m), sympy.Matrix.eye(4))
rot = matrices.TRotX(theta)
rot_1 = matrices.TRotX(-theta)
self.assertEqual(sympy.trigsimp(rot.m * rot_1.m), sympy.Matrix.eye(4))
def test_issue_4661():
a, x, y = symbols('a x y')
eq = -4*sin(x)**4 + 4*cos(x)**4 - 8*cos(x)**2
assert trigsimp(eq) == -4
n = sin(x)**6 + 4*sin(x)**4*cos(x)**2 + 5*sin(x)**2*cos(x)**4 + 2*cos(x)**6
d = -sin(x)**2 - 2*cos(x)**2
assert simplify(n/d) == -1
assert trigsimp(-2*cos(x)**2 + cos(x)**4 - sin(x)**4) == -1
eq = (- sin(x)**3/4)*cos(x) + (cos(x)**3/4)*sin(x) - sin(2*x)*cos(2*x)/8
assert trigsimp(eq) == 0
def _w_diff_dcm(self, otherframe):
"""Angular velocity from time differentiating the DCM. """
from sympy.physics.vector.functions import dynamicsymbols
dcm2diff = self.dcm(otherframe)
diffed = dcm2diff.diff(dynamicsymbols._t)
angvelmat = diffed * dcm2diff.T
w1 = trigsimp(expand(angvelmat[7]), recursive=True)
w2 = trigsimp(expand(angvelmat[2]), recursive=True)
w3 = trigsimp(expand(angvelmat[3]), recursive=True)
return -Vector([(Matrix([w1, w2, w3]), self)])
def solve_orientation(robo, symo, pieper_joints):
"""
Function that solves the orientation equation for the four spherical cases.
Parameters:
===========
1) pieper_joints: Joints that form the spherical wrist
"""
m = pieper_joints[1] # Center of the spherical joint
[S,N,A] = [0,1,2] # Book convention indexes
[x,y,z] = [0,1,2] # Book convention indexes
t1 = dgm(robo, symo, m-2, 0, fast_form=True, trig_subs=True)
t2 = dgm(robo, symo, 6, m+1, fast_form=True, trig_subs=True)
Am2A0 = Matrix([ t1[:3,:3] ])
A6Am1 = Matrix([ t2[:3,:3] ])
A0 = T_GENERAL[:3,:3]
SNA = _rot(axis=x, th=-robo.alpha[m-1])*Am2A0*A0*A6Am1
SNA = symo.replace(trigsimp(SNA), 'SNA')
# calculate theta(m-1) (i)
# eq 1.68
eq_type = 3
offset = robo.gamma[robo.ant[m-1]]
robo.r[m-1] = robo.r[m-1] + robo.b[robo.ant[m-1]]
coef = [-SNA[y,A]*sin(robo.alpha[m]) , SNA[x,A]*sin(robo.alpha[m]) , SNA[z,A]*cos(robo.alpha[m])-cos(robo.alpha[m+1])]
_equation_solve(symo, coef, eq_type, robo.theta[m-1], offset)
# calculate theta(m) (j)
# eq 1.72
S1N1A1 = _rot(axis=x, th=-robo.alpha[m])*_rot(axis=z, th=-robo.theta[m-1])*SNA
eq_type = 4
offset = robo.gamma[robo.ant[m]]
robo.r[m] = robo.r[m] + robo.b[robo.ant[m]]
symo.write_line("\r\n\r\n")
B1 = symo.replace(trigsimp(-S1N1A1[x,A]), 'B1', robo.theta[m])
B2 = symo.replace(trigsimp(S1N1A1[y,A]), 'B2', robo.theta[m])
coef = [0, sin(robo.alpha[m+1]), B1, sin(robo.alpha[m+1]), 0, B2]
_equation_solve(symo, coef, eq_type, robo.theta[m], offset)
# calculate theta(m+1) (k)
# eq 1.73
eq_type = 4
offset = robo.gamma[robo.ant[m+1]]
robo.r[m+1] = robo.r[m+1] + robo.b[robo.ant[m+1]]
symo.write_line("\r\n\r\n")
B1 = symo.replace(trigsimp(-S1N1A1[z,S]), 'B1', robo.theta[m+1])
B2 = symo.replace(trigsimp(-S1N1A1[z,N]), 'B2', robo.theta[m+1])
coef = [0, sin(robo.alpha[m+1]), B1, sin(robo.alpha[m+1]), 0, B2]
_equation_solve(symo, coef, eq_type, robo.theta[m+1], offset)
return
def _w_diff_dcm(self, otherframe):
"""Angular velocity from time differentiating the DCM. """
from sympy.physics.vector.functions import dynamicsymbols
dcm2diff = self.dcm(otherframe)
diffed = dcm2diff.diff(dynamicsymbols._t)
# angvelmat = diffed * dcm2diff.T
# This one seems to produce the correct result when I checked using Autolev.
angvelmat = dcm2diff*diffed.T
w1 = trigsimp(expand(angvelmat[7]), recursive=True)
w2 = trigsimp(expand(angvelmat[2]), recursive=True)
w3 = trigsimp(expand(angvelmat[3]), recursive=True)
return -Vector([(Matrix([w1, w2, w3]), self)])
def _try_solve_0(symo, eq_sys, knowns):
res = False
for eq, [r], th_names in eq_sys:
X = tools.get_max_coef(eq, r)
if X != 0:
Y = X*r - eq
symo.write_line("# Solving type 1")
X = symo.replace(trigsimp(X), 'X', r)
Y = symo.replace(trigsimp(Y), 'Y', r)
symo.add_to_dict(r, Y/X)
knowns.add(r)
res = True
return res
def _solve_type_4(symo, X1, Y1, X2, Y2, th, r):
"""Solution for the system:
X1*S*r = Y1
X2*C*r = Y2
"""
symo.write_line("# X1*sin({0})*{1} = Y1".format(th, r))
symo.write_line("# X2*cos({0})*{1} = Y2".format(th, r))
X1 = symo.replace(trigsimp(X1), 'X1', th)
Y1 = symo.replace(trigsimp(Y1), 'Y1', th)
X2 = symo.replace(trigsimp(X2), 'X2', th)
Y2 = symo.replace(trigsimp(Y2), 'Y2', th)
YPS = var('YPS' + r)
symo.add_to_dict(YPS, (tools.ONE, - tools.ONE))
symo.add_to_dict(r, YPS*sqrt((Y1/X1)**2 + (Y2/X2)**2))
symo.add_to_dict(th, atan2(Y1/(X1*r), Y2/(X2*r)))
def test_trigsimp3():
x, y = symbols('x,y')
assert trigsimp(sin(x)/cos(x)) == tan(x)
assert trigsimp(sin(x)**2/cos(x)**2) == tan(x)**2
assert trigsimp(sin(x)**3/cos(x)**3) == tan(x)**3
assert trigsimp(sin(x)**10/cos(x)**10) == tan(x)**10
assert trigsimp(cos(x)/sin(x)) == 1/tan(x)
assert trigsimp(cos(x)**2/sin(x)**2) == 1/tan(x)**2
assert trigsimp(cos(x)**10/sin(x)**10) == 1/tan(x)**10
assert trigsimp(tan(x)) == trigsimp(sin(x)/cos(x))
def square_root_of_expr(expr):
"""
If expression is product of even powers then every power is divided
by two and the product is returned. If some terms in product are
not even powers the sqrt of the absolute value of the expression is
returned. If the expression is a number the sqrt of the absolute
value of the number is returned.
"""
if expr.is_number:
if expr > 0:
return(sqrt(expr))
else:
return(sqrt(-expr))
else:
expr = trigsimp(expr)
(coef, pow_lst) = sqf_list(expr)
if coef != S(1):
if coef.is_number:
coef = square_root_of_expr(coef)
else:
coef = sqrt(abs(coef)) # Product coefficient not a number
for p in pow_lst:
(f, n) = p
if n % 2 != 0:
return(sqrt(abs(expr))) # Product not all even powers
else:
coef *= f ** (n / 2) # Positive sqrt of the square of an expression
return coef
def scalar_map(self, other):
"""
Returns a dictionary which expresses the coordinate variables
(base scalars) of this frame in terms of the variables of
otherframe.
Parameters
==========
otherframe : CoordSysCartesian
The other system to map the variables to.
Examples
========
>>> from sympy.vector import CoordSysCartesian
>>> from sympy import Symbol
>>> A = CoordSysCartesian('A')
>>> q = Symbol('q')
>>> B = A.orient_new('B', 'Axis', [q, A.k])
>>> A.scalar_map(B)
{A.x: B.x*cos(q) - B.y*sin(q), A.y: B.x*sin(q) + B.y*cos(q), A.z: B.z}
"""
vars_matrix = self.rotation_matrix(other) * Matrix(other.base_scalars())
mapping = {}
for i, x in enumerate(self.base_scalars()):
mapping[x] = trigsimp(vars_matrix[i])
return mapping
def _generate_eoms(self):
self.kane = me.KanesMethod(self.frames['inertial'],
self.coordinates.values(),
self.speeds.values(),
self.kin_diff_eqs)
fr, frstar = self.kane.kanes_equations(self.loads.values(),
self.rigid_bodies.values())
sub = {self.specified['platform_speed'].diff(self.time):
self.specified['platform_acceleration']}
self.fr_plus_frstar = sy.trigsimp(fr + frstar).subs(sub)
udots = sy.Matrix([s.diff(self.time) for s in self.speeds.values()])
m1 = self.fr_plus_frstar.diff(udots[0])
m2 = self.fr_plus_frstar.diff(udots[1])
# M x' = F
self.mass_matrix = -m1.row_join(m2)
self.forcing_vector = sy.simplify(self.fr_plus_frstar +
self.mass_matrix * udots)
M_top_rows = sy.eye(2).row_join(sy.zeros(2))
F_top_rows = sy.Matrix(self.speeds.values())
tmp = sy.zeros(2).row_join(self.mass_matrix)
self.mass_matrix_full = M_top_rows.col_join(tmp)
self.forcing_vector_full = F_top_rows.col_join(self.forcing_vector)
def solve_orientation_prismatic(robo, symo, X_joints):
"""
Function that solves the orientation equation of the three prismatic joints case. (to find the three angles)
Parameters:
===========
1) X_joint: The three revolute joints for the prismatic case
"""
[i,j,k] = X_joints # X joints vector
[S,S1,S2,S3,x] = [0,0,0,0,0] # Book convention indexes
[N,N1,N2,N3,y] = [1,1,1,1,1] # Book convention indexes
[A,A1,A2,A3,z] = [2,2,2,2,2] # Book convention indexes
robo.theta[i] = robo.theta[i] + robo.gamma[robo.ant[i]]
robo.theta[j] = robo.theta[j] + robo.gamma[robo.ant[j]]
robo.theta[k] = robo.theta[k] + robo.gamma[robo.ant[k]]
T6k = dgm(robo, symo, 6, k, fast_form=True,trig_subs=True)
Ti0 = dgm(robo, symo, robo.ant[i], 0, fast_form=True, trig_subs=True)
Tji = dgm(robo, symo, j-1, i, fast_form=True, trig_subs=True)
Tjk = dgm(robo, symo, j, k-1, fast_form=True, trig_subs=True)
S3N3A3 = _rot_trans(axis=x, th=-robo.alpha[i], p=0)*Ti0*T_GENERAL*T6k # S3N3A3 = rot(x,-alphai)*rot(z,-gami)*aiTo*SNA
S2N2A2 = _rot_trans(axis=x, th=-robo.alpha[j], p=0)*Tji # S2N2A2 = iTa(j)*rot(x,-alphaj)
S1N1A1 = Tjk*_rot_trans(axis=x, th=-robo.alpha[k], p=0) # S1N1A1 = jTa(k)*rot(x,alphak)
S3N3A3 = Matrix([ S3N3A3[:3, :3] ])
S2N2A2 = Matrix([ S2N2A2[:3, :3] ])
S1N1A1 = Matrix([ S1N1A1[:3, :3] ])
SNA = Matrix([ T_GENERAL[:3, :3] ])
SNA = symo.replace(trigsimp(SNA), 'SNA')
# solve thetai
# eq 1.100 (page 49)
eq_type = 3
offset = robo.gamma[robo.ant[i]]
robo.r[i] = robo.r[i] + robo.b[robo.ant[i]]
el1 = S2N2A2[z,S2]*S3N3A3[x,A3] + S2N2A2[z,N2]*S3N3A3[y,A3]
el2 = S2N2A2[z,S2]*S3N3A3[y,A3] - S2N2A2[z,N2]*S3N3A3[x,A3]
el3 = S2N2A2[z,A2]*S3N3A3[z,A3] - S1N1A1[z,A1]
coef = [el1,el2,el3]
_equation_solve(symo, coef, eq_type, robo.theta[i], offset)
# solve thetaj
# eq 1.102
eq_type = 4
offset = robo.gamma[robo.ant[j]]
robo.r[j] = robo.r[j] + robo.b[robo.ant[j]]
coef = [S1N1A1[x,A1] , -S1N1A1[y,A1] , -SNA[x,A] , S1N1A1[y,A1] , S1N1A1[x,A1] , -SNA[y,A] ]
_equation_solve(symo, coef, eq_type, robo.theta[j], offset)
# solve thetak
# eq 1.103
eq_type = 4
offset = robo.gamma[robo.ant[k]]
robo.r[k] = robo.r[k] + robo.b[robo.ant[k]]
coef = [S1N1A1[z,S1] , S1N1A1[z,N1] , -SNA[z,S] , S1N1A1[z,N1] , -S1N1A1[z,S1] , -SNA[z,N] ]
_equation_solve(symo, coef, eq_type, robo.theta[k], offset)
return
def _symplify_expr(self,expr): # this is a costly stage for complex expressions
"""
Perform simplification of the provided expression.
Method returns a SymPy expression.
"""
expr = sympy.trigsimp(expr)
expr = sympy.simplify(expr)
return expr
def test_issue_2827_trigsimp_methods():
measure1 = lambda expr: len(str(expr))
measure2 = lambda expr: -count_ops(expr)
# Return the most complicated result
expr = (x + 1)/(x + sin(x)**2 + cos(x)**2)
ans = Matrix([1])
M = Matrix([expr])
assert trigsimp(M, method='fu', measure=measure1) == ans
assert trigsimp(M, method='fu', measure=measure2) != ans
# all methods should work with Basic expressions even if they
# aren't Expr
M = Matrix.eye(1)
assert all(trigsimp(M, method=m) == M for m in
'fu matching groebner old'.split())
# watch for E in exptrigsimp, not only exp()
eq = 1/sqrt(E) + E
assert exptrigsimp(eq) == eq
def test_issue_6811_fail():
# from doc/src/modules/physics/mechanics/examples.rst, the current `eq`
# at Line 576 (in different variables) was formerly the equivalent and
# shorter expression given below...it would be nice to get the short one
# back again
xp, y, x, z = symbols('xp, y, x, z')
eq = 4*(-19*sin(x)*y + 5*sin(3*x)*y + 15*cos(2*x)*z - 21*z)*xp/(9*cos(x) - 5*cos(3*x))
assert trigsimp(eq) == -2*(2*cos(x)*tan(x)*y + 3*z)*xp/cos(x)
def half_angle_reduce(expr, theta):
s, c = symbols('s c')
sub_dict = {sin(theta / 2): s, cos(theta / 2): c}
new_expr = expr.subs(sub_dict)
sub_dict = {s * c: sin(theta) / 2, s**2: (1 - cos(theta)) / 2, c**2: (1 + cos(theta)) / 2}
# print new_expr
new_expr = trigsimp(simplify(new_expr.subs(sub_dict)), recursive=True)
# print expand(new_expr)
return new_expr
def to_axis_angle(self):
"""Returns the axis and angle of rotation of a quaternion
Returns
=======
tuple
Tuple of (axis, angle)
Examples
========
>>> from sympy.algebras.quaternion import Quaternion
>>> q = Quaternion(1, 1, 1, 1)
>>> (axis, angle) = q.to_axis_angle()
>>> axis
(sqrt(3)/3, sqrt(3)/3, sqrt(3)/3)
>>> angle
2*pi/3
"""
q = self
try:
# Skips it if it doesn't know whether q.a is negative
if q.a < 0:
# avoid error with acos
# axis and angle of rotation of q and q*-1 will be the same
q = q * -1
except BaseException:
pass
q = q.normalize()
angle = trigsimp(2 * acos(q.a))
# Since quaternion is normalised, q.a is less than 1.
s = sqrt(1 - q.a*q.a)
x = trigsimp(q.b / s)
y = trigsimp(q.c / s)
z = trigsimp(q.d / s)
v = (x, y, z)
t = (v, angle)
return t
def test_hm_inverse_x_sorting(self):
theta = sympy.Symbol("theta")
alpha = sympy.Symbol("alpha")
r = sympy.Symbol("r")
d = sympy.Symbol("d")
hm = matrices.HomMatrix(alpha, r, d, theta)
self.assertEqual(sympy.trigsimp(hm.m * hm.m_1), sympy.Matrix.eye(4))
def add_joint(self, joint):
self.joints.append(joint)
self.joints_sym.append(joint)
self.constants += joint.constants
self.params += joint.params
self.hms.append(self.hms[-1]*joint.m)
if self.simplify:
self.hms[-1] = sympy.trigsimp(self.hms[-1])
self.hms_sym.append(self.hms[-1])
self.functional = False
def test_issue_4892b():
# Issues relating to issue 4596 are making the actual result of this hard
# to test. The answer should be something like
#
# (-sin(y) + sqrt(-72 + 48*cos(y) - 8*cos(y)**2)/2)*log(x + sqrt(-72 +
# 48*cos(y) - 8*cos(y)**2)/(2*(3 - cos(y)))) + (-sin(y) - sqrt(-72 +
# 48*cos(y) - 8*cos(y)**2)/2)*log(x - sqrt(-72 + 48*cos(y) -
# 8*cos(y)**2)/(2*(3 - cos(y)))) + x**2*sin(y)/2 + 2*x*cos(y)
expr = (sin(y)*x**3 + 2*cos(y)*x**2 + 12)/(x**2 + 2)
assert trigsimp(factor(integrate(expr, x).diff(x) - expr)) == 0
def test_Piecewise():
e1 = x*(x + y) - y*(x + y)
e2 = sin(x)**2 + cos(x)**2
e3 = expand((x + y)*y/x)
s1 = simplify(e1)
s2 = simplify(e2)
s3 = simplify(e3)
# trigsimp tries not to touch non-trig containing args
assert trigsimp(Piecewise((e1, e3 < e2), (e3, True))) == \
Piecewise((e1, e3 < s2), (e3, True))
def coupled_speeds(ic_matrix, partials):
ulist = partials.ulist
print('dynamically coupled speeds:')
found = False
for i, r in enumerate(ulist):
for j, s in enumerate(ulist[i + 1:], i + 1):
if trigsimp(ic_matrix[i, j]) != 0:
found = True
print('{0} and {1}'.format(msprint(r), msprint(s)))
if not found:
print('None')
def apply(self, expr, evaluation):
'Simplify[expr_]'
expr_sympy = expr.to_sympy()
result = expr_sympy
result = sympy.simplify(result)
result = sympy.trigsimp(result)
result = sympy.together(result)
result = sympy.cancel(result)
result = from_sympy(result)
return result
def test_quaternion_construction():
q = Quaternion(x, y, z, w)
assert q + q == Quaternion(2*x, 2*y, 2*z, 2*w)
q2 = Quaternion.from_axis_angle((sqrt(3)/3, sqrt(3)/3, sqrt(3)/3), 2*pi/3)
assert q2 == Quaternion(Rational(1/2), Rational(1/2),
Rational(1/2), Rational(1,2))
M = Matrix([[cos(x), -sin(x), 0], [sin(x), cos(x), 0], [0, 0, 1]])
q3 = trigsimp(Quaternion.from_rotation_matrix(M))
assert q3 == Quaternion(sqrt(2)*sqrt(cos(x) + 1)/2, 0, 0, sqrt(-2*cos(x) + 2)/2)
def _solve_type_8(symo, X, Y, Z1, Z2, th_i, th_j):
"""Solution for the system:
X*Ci + Y*Cij = Z1
X*Si + Y*Sij = Z2
"""
symo.write_line("# X*cos({0}) + Y*cos({0} + {1}) = Z1".format(th_i, th_j))
symo.write_line("# X*sin({0}) + Y*sin({0} + {1}) = Z2".format(th_i, th_j))
X = symo.replace(trigsimp(X), 'X', th_j)
Y = symo.replace(trigsimp(Y), 'Y', th_j)
Z1 = symo.replace(trigsimp(Z1), 'Z1', th_j)
Z2 = symo.replace(trigsimp(Z2), 'Z2', th_j)
Cj = symo.replace((Z1**2 + Z2**2 - X**2 - Y**2) / (2*X*Y), 'C', th_j)
YPS = var('YPS%s' % th_j)
symo.add_to_dict(YPS, (tools.ONE, -tools.ONE))
symo.add_to_dict(th_j, atan2(YPS*sqrt(1 - Cj**2), Cj))
Q1 = symo.replace(X + Y*cos(th_j), 'Q1', th_i)
Q2 = symo.replace(Y*sin(th_j), 'Q2', th_i)
Den = symo.replace(Q1**2 + Q2**2, 'Den', th_i)
Si = symo.replace((Q1*Z2 - Q2*Z1)/Den, 'S', th_i)
Ci = symo.replace((Q1*Z1 + Q2*Z2)/Den, 'C', th_i)
symo.add_to_dict(th_i, atan2(Si, Ci))
def test_trigsimp_issue_4032():
n = Symbol('n', integer=True, positive=True)
assert trigsimp(2**(n/2)*cos(pi*n/4)/2 + 2**(n - 1)/2) == \
2**(n/2)*cos(pi*n/4)/2 + 2**n/4
def test_trigsimp_noncommutative():
x, y = symbols('x,y')
A, B = symbols('A,B', commutative=False)
assert trigsimp(A - A * sin(x)**2) == A * cos(x)**2
assert trigsimp(A - A * cos(x)**2) == A * sin(x)**2
assert trigsimp(A * sin(x)**2 + A * cos(x)**2) == A
assert trigsimp(A + A * tan(x)**2) == A / cos(x)**2
assert trigsimp(A / cos(x)**2 - A) == A * tan(x)**2
assert trigsimp(A / cos(x)**2 - A * tan(x)**2) == A
assert trigsimp(A + A * cot(x)**2) == A / sin(x)**2
assert trigsimp(A / sin(x)**2 - A) == A / tan(x)**2
assert trigsimp(A / sin(x)**2 - A * cot(x)**2) == A
assert trigsimp(y * A * cos(x)**2 + y * A * sin(x)**2) == y * A
assert trigsimp(A * sin(x) / cos(x)) == A * tan(x)
assert trigsimp(A * tan(x) * cos(x)) == A * sin(x)
assert trigsimp(A * cot(x)**3 * sin(x)**3) == A * cos(x)**3
assert trigsimp(y * A * tan(x)**2 / sin(x)**2) == y * A / cos(x)**2
assert trigsimp(A * cot(x) / cos(x)) == A / sin(x)
assert trigsimp(A * sin(x + y) + A * sin(x - y)) == 2 * A * sin(x) * cos(y)
assert trigsimp(A * sin(x + y) - A * sin(x - y)) == 2 * A * sin(y) * cos(x)
assert trigsimp(A * cos(x + y) + A * cos(x - y)) == 2 * A * cos(x) * cos(y)
assert trigsimp(A * cos(x + y) -
A * cos(x - y)) == -2 * A * sin(x) * sin(y)
assert trigsimp(A * sinh(x + y) +
A * sinh(x - y)) == 2 * A * sinh(x) * cosh(y)
assert trigsimp(A * sinh(x + y) -
A * sinh(x - y)) == 2 * A * sinh(y) * cosh(x)
assert trigsimp(A * cosh(x + y) +
A * cosh(x - y)) == 2 * A * cosh(x) * cosh(y)
assert trigsimp(A * cosh(x + y) -
A * cosh(x - y)) == 2 * A * sinh(x) * sinh(y)
assert trigsimp(A * cos(0.12345)**2 + A * sin(0.12345)**2) == 1.0 * A
def test_trigsimp_issue_7761():
assert trigsimp(cosh(pi / 4)) == cosh(pi / 4)
def test_issue_4775():
a, x, y = symbols('a x y')
assert trigsimp(sin(x) * cos(y) + cos(x) * sin(y)) == sin(x + y)
assert trigsimp(sin(x) * cos(y) + cos(x) * sin(y) + 3) == sin(x + y) + 3
def test_sub_qdot2():
# This test solves exercises 8.3 from Kane 1985 and defines
# all velocities in terms of q, qdot. We check that the generalized active
# forces are correctly computed if u terms are only defined in the
# kinematic differential equations.
#
# This functionality was added in PR 8948. Without qdot/u substitution, the
# KanesMethod constructor will fail during the constraint initialization as
# the B matrix will be poorly formed and inversion of the dependent part
# will fail.
g, m, Px, Py, Pz, R, t = symbols('g m Px Py Pz R t')
q = dynamicsymbols('q:5')
qd = dynamicsymbols('q:5', level=1)
u = dynamicsymbols('u:5')
## Define inertial, intermediate, and rigid body reference frames
A = ReferenceFrame('A')
B_prime = A.orientnew('B_prime', 'Axis', [q[0], A.z])
B = B_prime.orientnew('B', 'Axis', [pi / 2 - q[1], B_prime.x])
C = B.orientnew('C', 'Axis', [q[2], B.z])
## Define points of interest and their velocities
pO = Point('O')
pO.set_vel(A, 0)
# R is the point in plane H that comes into contact with disk C.
pR = pO.locatenew('R', q[3] * A.x + q[4] * A.y)
pR.set_vel(A, pR.pos_from(pO).diff(t, A))
pR.set_vel(B, 0)
# C^ is the point in disk C that comes into contact with plane H.
pC_hat = pR.locatenew('C^', 0)
pC_hat.set_vel(C, 0)
# C* is the point at the center of disk C.
pCs = pC_hat.locatenew('C*', R * B.y)
pCs.set_vel(C, 0)
pCs.set_vel(B, 0)
# calculate velocites of points C* and C^ in frame A
pCs.v2pt_theory(pR, A, B) # points C* and R are fixed in frame B
pC_hat.v2pt_theory(pCs, A, C) # points C* and C^ are fixed in frame C
## Define forces on each point of the system
R_C_hat = Px * A.x + Py * A.y + Pz * A.z
R_Cs = -m * g * A.z
forces = [(pC_hat, R_C_hat), (pCs, R_Cs)]
## Define kinematic differential equations
# let ui = omega_C_A & bi (i = 1, 2, 3)
# u4 = qd4, u5 = qd5
u_expr = [C.ang_vel_in(A) & uv for uv in B]
u_expr += qd[3:]
kde = [ui - e for ui, e in zip(u, u_expr)]
km1 = KanesMethod(A, q, u, kde)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
fr1, _ = km1.kanes_equations(forces, [])
## Calculate generalized active forces if we impose the condition that the
# disk C is rolling without slipping
u_indep = u[:3]
u_dep = list(set(u) - set(u_indep))
vc = [pC_hat.vel(A) & uv for uv in [A.x, A.y]]
km2 = KanesMethod(A,
q,
u_indep,
kde,
u_dependent=u_dep,
velocity_constraints=vc)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
fr2, _ = km2.kanes_equations(forces, [])
fr1_expected = Matrix([
-R * g * m * sin(q[1]),
-R * (Px * cos(q[0]) + Py * sin(q[0])) * tan(q[1]),
R * (Px * cos(q[0]) + Py * sin(q[0])), Px, Py
])
fr2_expected = Matrix([-R * g * m * sin(q[1]), 0, 0])
assert (trigsimp(fr1.expand()) == trigsimp(fr1_expected.expand()))
assert (trigsimp(fr2.expand()) == trigsimp(fr2_expected.expand()))
def test_L4():
assert trigsimp(cos(x)**3 + cos(x) * sin(x)**2 - cos(x)) == 0
def test_trigsimp_issues():
a, x, y = symbols('a x y')
# issue 4625 - factor_terms works, too
assert trigsimp(sin(x)**3 + cos(x)**2 * sin(x)) == sin(x)
# issue 5948
assert trigsimp(diff(integrate(cos(x)/sin(x)**3, x), x)) == \
cos(x)/sin(x)**3
assert trigsimp(diff(integrate(sin(x)/cos(x)**3, x), x)) == \
sin(x)/cos(x)**3
# check integer exponents
e = sin(x)**y / cos(x)**y
assert trigsimp(e) == e
assert trigsimp(e.subs(y, 2)) == tan(x)**2
assert trigsimp(e.subs(x, 1)) == tan(1)**y
# check for multiple patterns
assert (cos(x)**2/sin(x)**2*cos(y)**2/sin(y)**2).trigsimp() == \
1/tan(x)**2/tan(y)**2
assert trigsimp(cos(x)/sin(x)*cos(x+y)/sin(x+y)) == \
1/(tan(x)*tan(x + y))
eq = cos(2) * (cos(3) + 1)**2 / (cos(3) - 1)**2
assert trigsimp(eq) == eq.factor() # factor makes denom (-1 + cos(3))**2
assert trigsimp(cos(2)*(cos(3) + 1)**2*(cos(3) - 1)**2) == \
cos(2)*sin(3)**4
# issue 6789; this generates an expression that formerly caused
# trigsimp to hang
assert cot(x).equals(tan(x)) is False
# nan or the unchanged expression is ok, but not sin(1)
z = cos(x)**2 + sin(x)**2 - 1
z1 = tan(x)**2 - 1 / cot(x)**2
n = (1 + z1 / z)
assert trigsimp(sin(n)) != sin(1)
eq = x * (n - 1) - x * n
assert trigsimp(eq) is S.NaN
assert trigsimp(eq, recursive=True) is S.NaN
assert trigsimp(1).is_Integer
assert trigsimp(-sin(x)**4 - 2 * sin(x)**2 * cos(x)**2 - cos(x)**4) == -1
def get_symbolic_equations_of_motion(self, verbose=False):
"""
This returns the symbolic equation of motions of the robot (using the URDF). Internally, this used the
`sympy.mechanics` module.
"""
# gravity and time
g, t = sympy.symbols('g t')
# create the world inertial frame of reference and its origin
world_frame = mechanics.ReferenceFrame('Fw')
world_origin = mechanics.Point('Pw')
world_origin.set_vel(world_frame, mechanics.Vector(0))
# create the base frame (its position, orientation and velocities) + generalized coordinates and speeds
base_id = -1
# Check if the robot has a fixed base and create the generalized coordinates and speeds based on that,
# as well the base position, orientation and velocities
if self.has_fixed_base():
# generalized coordinates q(t) and speeds dq(t)
q = mechanics.dynamicsymbols('q:{}'.format(len(self.joints)))
dq = mechanics.dynamicsymbols('dq:{}'.format(len(self.joints)))
pos, orn = self.get_base_pose()
lin_vel, ang_vel = [0, 0, 0], [0, 0, 0] # 0 because fixed base
joint_id = 0
else:
# generalized coordinates q(t) and speeds dq(t)
q = mechanics.dynamicsymbols('q:{}'.format(7 + len(self.joints)))
dq = mechanics.dynamicsymbols('dq:{}'.format(6 + len(self.joints)))
pos, orn = q[:3], q[3:7]
lin_vel, ang_vel = dq[:3], dq[3:6]
joint_id = 7
# set the position, orientation and velocities of the base
base_frame = world_frame.orientnew('Fb', 'Quaternion',
[orn[3], orn[0], orn[1], orn[2]])
base_frame.set_ang_vel(
world_frame, ang_vel[0] * world_frame.x +
ang_vel[1] * world_frame.y + ang_vel[2] * world_frame.z)
base_origin = world_origin.locatenew(
'Pb', pos[0] * world_frame.x + pos[1] * world_frame.y +
pos[2] * world_frame.z)
base_origin.set_vel(
world_frame, lin_vel[0] * world_frame.x +
lin_vel[1] * world_frame.y + lin_vel[2] * world_frame.z)
# inputs u(t) (applied torques)
u = mechanics.dynamicsymbols('u:{}'.format(len(self.joints)))
joint_id_u = 0
# kinematics differential equations
kd_eqs = [q[i].diff(t) - dq[i] for i in range(len(self.joints))]
# define useful lists/dicts for later
bodies, loads = [], []
frames = {base_id: (base_frame, base_origin)}
# frames = {base_id: (worldFrame, worldOrigin)}
# go through each joint/link (each link is associated to a joint)
for link_id in range(self.num_links):
# get useful information about joint/link kinematics and dynamics from simulator
info = self.sim.get_dynamics_info(self.id, link_id)
mass, local_inertia_diagonal = info[0], info[2]
info = self.sim.get_link_state(self.id, link_id)
local_inertial_frame_position, local_inertial_frame_orientation = info[
2], info[3]
# worldLinkFramePosition, worldLinkFrameOrientation = info[4], info[5]
info = self.sim.get_joint_info(self.id, link_id)
joint_name, joint_type = info[1:3]
# jointDamping, jointFriction = info[6:8]
link_name, joint_axis_in_local_frame, parent_frame_position, parent_frame_orientation, \
parent_idx = info[-5:]
xl, yl, zl = joint_axis_in_local_frame
# get previous references
parent_frame, parent_point = frames[parent_idx]
# create a reference frame with its origin for each joint
# set frame orientation
if joint_type == self.sim.JOINT_REVOLUTE:
R = get_matrix_from_quaternion(parent_frame_orientation)
R1 = get_symbolic_matrix_from_axis_angle(
joint_axis_in_local_frame, q[joint_id])
R = R1.dot(R)
frame = parent_frame.orientnew('F' + str(link_id), 'DCM',
sympy.Matrix(R))
else:
x, y, z, w = parent_frame_orientation # orientation of the joint in parent CoM inertial frame
frame = parent_frame.orientnew('F' + str(link_id),
'Quaternion', [w, x, y, z])
# set frame angular velocity
ang_vel = 0
if joint_type == self.sim.JOINT_REVOLUTE:
ang_vel = dq[joint_id] * (xl * frame.x + yl * frame.y +
zl * frame.z)
frame.set_ang_vel(parent_frame, ang_vel)
# create origin of the reference frame
# set origin position
x, y, z = parent_frame_position # position of the joint in parent CoM inertial frame
pos = x * parent_frame.x + y * parent_frame.y + z * parent_frame.z
if joint_type == self.sim.JOINT_PRISMATIC:
pos += q[joint_id] * (xl * frame.x + yl * frame.y +
zl * frame.z)
origin = parent_point.locatenew('P' + str(link_id), pos)
# set origin velocity
if joint_type == self.sim.JOINT_PRISMATIC:
vel = dq[joint_id] * (xl * frame.x + yl * frame.y +
zl * frame.z)
origin.set_vel(world_frame, vel.express(world_frame))
else:
origin.v2pt_theory(parent_point, world_frame, parent_frame)
# define CoM frame and position (and velocities) wrt the local link frame
x, y, z, w = local_inertial_frame_orientation
com_frame = frame.orientnew('Fc' + str(link_id), 'Quaternion',
[w, x, y, z])
com_frame.set_ang_vel(frame, mechanics.Vector(0))
x, y, z = local_inertial_frame_position
com = origin.locatenew('C' + str(link_id),
x * frame.x + y * frame.y + z * frame.z)
com.v2pt_theory(origin, world_frame, frame)
# define com particle
# com_particle = mechanics.Particle('Pa' + str(linkId), com, mass)
# bodies.append(com_particle)
# save
# frames[linkId] = (frame, origin)
# frames[linkId] = (frame, origin, com_frame, com)
frames[link_id] = (com_frame, com)
# define mass and inertia
ixx, iyy, izz = local_inertia_diagonal
inertia = mechanics.inertia(com_frame,
ixx,
iyy,
izz,
ixy=0,
iyz=0,
izx=0)
inertia = (inertia, com)
# define rigid body associated to frame
body = mechanics.RigidBody(link_name, com, frame, mass, inertia)
bodies.append(body)
# define dynamical forces/torques acting on the body
# gravity force applied on the CoM
force = (com, -mass * g * world_frame.z)
loads.append(force)
# if prismatic joint, compute force
if joint_type == self.sim.JOINT_PRISMATIC:
force = (origin, u[joint_id_u] *
(xl * frame.x + yl * frame.y + zl * frame.z))
# force = (com, u[jointIdU] * (x * frame.x + y * frame.y + z * frame.z) - mass * g * worldFrame.z)
loads.append(force)
# if revolute joint, compute torque
if joint_type == self.sim.JOINT_REVOLUTE:
v = (xl * frame.x + yl * frame.y + zl * frame.z)
# torqueOnPrevBody = (parentFrame, - u[jointIdU] * v)
torque_on_prev_body = (parent_frame, -u[joint_id_u] * v)
torque_on_curr_body = (frame, u[joint_id_u] * v)
loads.append(torque_on_prev_body)
loads.append(torque_on_curr_body)
# if joint is not fixed increment the current joint id
if joint_type != self.sim.JOINT_FIXED:
joint_id += 1
joint_id_u += 1
if verbose:
print("\nLink name with type: {} - {}".format(
link_name, self.get_joint_types(joint_ids=link_id)))
print("------------------------------------------------------")
print("Position of joint frame wrt parent frame: {}".format(
origin.pos_from(parent_point)))
print("Orientation of joint frame wrt parent frame: {}".format(
frame.dcm(parent_frame)))
print("Linear velocity of joint frame wrt parent frame: {}".
format(origin.vel(world_frame).express(parent_frame)))
print("Angular velocity of joint frame wrt parent frame: {}".
format(frame.ang_vel_in(parent_frame)))
print("------------------------------------------------------")
print("Position of joint frame wrt world frame: {}".format(
origin.pos_from(world_origin)))
print("Orientation of joint frame wrt world frame: {}".format(
frame.dcm(world_frame).simplify()))
print("Linear velocity of joint frame wrt world frame: {}".
format(origin.vel(world_frame)))
print("Angular velocity of joint frame wrt parent frame: {}".
format(frame.ang_vel_in(world_frame)))
print("------------------------------------------------------")
# print("Local position of CoM wrt joint frame: {}".format(com.pos_from(origin)))
# print("Local linear velocity of CoM wrt joint frame: {}".format(com.vel(worldFrame).express(frame)))
# print("Local angular velocity of CoM wrt joint frame: {}".format(com_frame.ang_vel_in(frame)))
# print("------------------------------------------------------")
if joint_type == self.sim.JOINT_PRISMATIC:
print("Input value (force): {}".format(loads[-1]))
elif joint_type == self.sim.JOINT_REVOLUTE:
print(
"Input value (torque on previous and current bodies): {} and {}"
.format(loads[-2], loads[-1]))
print("")
if verbose:
print("Summary:")
print("Generalized coordinates: {}".format(q))
print("Generalized speeds: {}".format(dq))
print("Inputs: {}".format(u))
print("Kinematic differential equations: {}".format(kd_eqs))
print("Bodies: {}".format(bodies))
print("Loads: {}".format(loads))
print("")
# TODO: 1. account for external forces applied on different rigid-bodies (e.g. contact forces)
# TODO: 2. account for constraints (e.g. holonomic, non-holonomic, etc.)
# Get the Equation of Motion (EoM) using Kane's method
kane = mechanics.KanesMethod(world_frame,
q_ind=q,
u_ind=dq,
kd_eqs=kd_eqs)
kane.kanes_equations(bodies=bodies, loads=loads)
# get mass matrix and force vector (after simplifying) such that :math:`M(x,t) \dot{x} = f(x,t)`
M = sympy.trigsimp(kane.mass_matrix_full)
f = sympy.trigsimp(kane.forcing_full)
# mechanics.find_dynamicsymbols(M)
# mechanics.find_dynamicsymbols(f)
# save useful info for future use (by other methods)
constants = [g]
constant_values = [9.81]
parameters = (dict(zip(constants, constant_values)))
self.symbols = {
'q': q,
'dq': dq,
'kane': kane,
'parameters': parameters
}
# linearize
# parameters = dict(zip(constants, constant_values))
# M_, A_, B_, u_ = kane.linearize()
# A_ = A_.subs(parameters)
# B_ = B_.subs(parameters)
# M_ = kane.mass_matrix_full.subs(parameters)
# self.symbols = {'A_': A_, 'B_': B_, 'M_': M_}
# return M_, A_, B_, u_
return M, f
def test_trigsimp():
assert trigsimp(A * sin(x)**2 + A * cos(x)**2) == A
def test_trigsimp1a():
assert trigsimp(sin(2)**2 * cos(3) * exp(2) /
cos(2)**2) == tan(2)**2 * cos(3) * exp(2)
assert trigsimp(tan(2)**2 * cos(3) * exp(2) *
cos(2)**2) == sin(2)**2 * cos(3) * exp(2)
assert trigsimp(cot(2) * cos(3) * exp(2) *
sin(2)) == cos(3) * exp(2) * cos(2)
assert trigsimp(tan(2) * cos(3) * exp(2) /
sin(2)) == cos(3) * exp(2) / cos(2)
assert trigsimp(cot(2) * cos(3) * exp(2) /
cos(2)) == cos(3) * exp(2) / sin(2)
assert trigsimp(cot(2) * cos(3) * exp(2) * tan(2)) == cos(3) * exp(2)
assert trigsimp(sinh(2) * cos(3) * exp(2) /
cosh(2)) == tanh(2) * cos(3) * exp(2)
assert trigsimp(tanh(2) * cos(3) * exp(2) *
cosh(2)) == sinh(2) * cos(3) * exp(2)
assert trigsimp(coth(2) * cos(3) * exp(2) *
sinh(2)) == cosh(2) * cos(3) * exp(2)
assert trigsimp(tanh(2) * cos(3) * exp(2) /
sinh(2)) == cos(3) * exp(2) / cosh(2)
assert trigsimp(coth(2) * cos(3) * exp(2) /
cosh(2)) == cos(3) * exp(2) / sinh(2)
assert trigsimp(coth(2) * cos(3) * exp(2) * tanh(2)) == cos(3) * exp(2)
def test_trigsimp1():
x, y = symbols('x,y')
assert trigsimp(1 - sin(x)**2) == cos(x)**2
assert trigsimp(1 - cos(x)**2) == sin(x)**2
assert trigsimp(sin(x)**2 + cos(x)**2) == 1
assert trigsimp(1 + tan(x)**2) == 1 / cos(x)**2
assert trigsimp(1 / cos(x)**2 - 1) == tan(x)**2
assert trigsimp(1 / cos(x)**2 - tan(x)**2) == 1
assert trigsimp(1 + cot(x)**2) == 1 / sin(x)**2
assert trigsimp(1 / sin(x)**2 - 1) == cot(x)**2
assert trigsimp(1 / sin(x)**2 - cot(x)**2) == 1
assert trigsimp(5 * cos(x)**2 + 5 * sin(x)**2) == 5
assert trigsimp(5*cos(x/2)**2 + 2*sin(x/2)**2) in \
[2 + 3*cos(x/2)**2, 5 - 3*sin(x/2)**2]
assert trigsimp(sin(x) / cos(x)) == tan(x)
assert trigsimp(2 * tan(x) * cos(x)) == 2 * sin(x)
assert trigsimp(cot(x)**3 * sin(x)**3) == cos(x)**3
assert trigsimp(y * tan(x)**2 / sin(x)**2) == y / cos(x)**2
assert trigsimp(cot(x) / cos(x)) == 1 / sin(x)
assert trigsimp(cos(0.12345)**2 + sin(0.12345)**2) == 1
e = 2 * sin(x)**2 + 2 * cos(x)**2
assert trigsimp(log(e), deep=True) == log(2)
def proper_simplify(expr):
""" Combine trig and normal simplification for sympy expression."""
return sympy.simplify(sympy.trigsimp(expr))
pC_hat.set_vel(C, 0)
# C* is the point at the center of disk C.
pC_star = pC_hat.locatenew('C*', R * B.y)
pC_star.set_vel(C, 0)
pC_star.set_vel(B, 0)
# calculate velocities in A
pC_star.v2pt_theory(pR, A, B)
pC_hat.v2pt_theory(pC_star, A, C)
## --- Expressions for generalized speeds u1, u2, u3, u4, u5 ---
u_expr = map(lambda x: dot(C.ang_vel_in(A), x), B)
u_expr += qd[3:]
kde = [u_i - u_ex for u_i, u_ex in zip(u, u_expr)]
kde_map = solve(kde, qd)
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))
# kinetic energy
K = collect(trigsimp(rbC.kinetic_energy(A).subs(kde_map).subs(vc_map)),
m * R**2 / 8)
print('K = {0}'.format(msprint(K)))
K_expected = (m * R**2 / 8) * (5 * u1**2 + u2**2 + 6 * u3**2)
assert expand(K - K_expected) == 0
def orient_new(self,
name,
orienters,
location=None,
vector_names=None,
variable_names=None):
"""
Creates a new CoordSys3D oriented in the user-specified way
with respect to this system.
Please refer to the documentation of the orienter classes
for more information about the orientation procedure.
Parameters
==========
name : str
The name of the new CoordSys3D instance.
orienters : iterable/Orienter
An Orienter or an iterable of Orienters for orienting the
new coordinate system.
If an Orienter is provided, it is applied to get the new
system.
If an iterable is provided, the orienters will be applied
in the order in which they appear in the iterable.
location : Vector(optional)
The location of the new coordinate system's origin wrt this
system's origin. If not specified, the origins are taken to
be coincident.
vector_names, variable_names : iterable(optional)
Iterables of 3 strings each, with custom names for base
vectors and base scalars of the new system respectively.
Used for simple str printing.
Examples
========
>>> from sympy.vector import CoordSys3D
>>> from sympy import symbols
>>> q0, q1, q2, q3 = symbols('q0 q1 q2 q3')
>>> N = CoordSys3D('N')
Using an AxisOrienter
>>> from sympy.vector import AxisOrienter
>>> axis_orienter = AxisOrienter(q1, N.i + 2 * N.j)
>>> A = N.orient_new('A', (axis_orienter, ))
Using a BodyOrienter
>>> from sympy.vector import BodyOrienter
>>> body_orienter = BodyOrienter(q1, q2, q3, '123')
>>> B = N.orient_new('B', (body_orienter, ))
Using a SpaceOrienter
>>> from sympy.vector import SpaceOrienter
>>> space_orienter = SpaceOrienter(q1, q2, q3, '312')
>>> C = N.orient_new('C', (space_orienter, ))
Using a QuaternionOrienter
>>> from sympy.vector import QuaternionOrienter
>>> q_orienter = QuaternionOrienter(q0, q1, q2, q3)
>>> D = N.orient_new('D', (q_orienter, ))
"""
if variable_names is None:
variable_names = self._variable_names
if vector_names is None:
vector_names = self._vector_names
if isinstance(orienters, Orienter):
if isinstance(orienters, AxisOrienter):
final_matrix = orienters.rotation_matrix(self)
else:
final_matrix = orienters.rotation_matrix()
# TODO: trigsimp is needed here so that the matrix becomes
# canonical (scalar_map also calls trigsimp; without this, you can
# end up with the same CoordinateSystem that compares differently
# due to a differently formatted matrix). However, this is
# probably not so good for performance.
final_matrix = trigsimp(final_matrix)
else:
final_matrix = Matrix(eye(3))
for orienter in orienters:
if isinstance(orienter, AxisOrienter):
final_matrix *= orienter.rotation_matrix(self)
else:
final_matrix *= orienter.rotation_matrix()
return CoordSys3D(name,
rotation_matrix=final_matrix,
vector_names=vector_names,
variable_names=variable_names,
location=location,
parent=self)
pP = pO.locatenew('P', q1 * A.x + q2 * A.y + q3 * A.z)
pP.set_vel(A, pP.pos_from(pO).dt(A))
# kinematic differential equations
kde_map = dict(zip(map(lambda x: x.diff(), q), u))
# forces
forces = [(pP, -beta * pP.vel(A))]
torques = [(B, -alpha * B.ang_vel_in(A))]
partials_c = partial_velocities(zip(*forces + torques)[0], u, A, kde_map)
Fr_c, _ = generalized_active_forces(partials_c, forces + torques)
dissipation_function = function_from_partials(map(
lambda x: 0 if x == 0 else -x.subs(kde_map), Fr_c),
u,
zero_constants=True)
from sympy import trigsimp
dissipation_function = trigsimp(dissipation_function)
#print('ℱ = {0}'.format(msprint(dissipation_function)))
omega2 = trigsimp(dot(B.ang_vel_in(A), B.ang_vel_in(A)).subs(kde_map))
v2 = trigsimp(dot(pP.vel(A), pP.vel(A)).subs(kde_map))
sym_map = dict(zip([omega2, v2], map(lambda x: x**2, symbols('ω v'))))
#print('ω**2 = {0}'.format(msprint(omega2)))
#print('v**2 = {0}'.format(msprint(v2)))
print('ℱ = {0}'.format(msprint(dissipation_function.subs(sym_map))))
dissipation_function_expected = (alpha * omega2 + beta * v2) / 2
assert expand(dissipation_function - dissipation_function_expected) == 0
def Product_of_Rotors():
Print_Function()
(na,nb,nm,alpha,th,th_a,th_b) = symbols('n_a n_b n_m alpha theta theta_a theta_b',\
real = True)
g = [[na, 0, alpha], [0, nm, 0], [alpha, 0, nb]] #metric tensor
"""
Values of metric tensor components
[na,nm,nb] = [+1/-1,+1/-1,+1/-1] alpha = ea|eb
"""
(g3d, ea, em, eb) = Ga.build('e_a e_m e_b', g=g)
print 'g =', g3d.g
print r'%n_{a} = \bm{e}_{a}^{2}\;\;n_{b} = \bm{e}_{b}^{2}\;\;n_{m} = \bm{e}_{m}^{2}'+\
r'\;\;\alpha = \bm{e}_{a}\cdot\bm{e}_{b}'
(ca, cb, sa, sb) = symbols('c_a c_b s_a s_b', real=True)
Ra = ca + sa * ea * em # Rotor for ea^em plane
Rb = cb + sb * em * eb # Rotor for em^eb plane
print r'%\mbox{Rotor in }\bm{e}_{a}\bm{e}_{m}\mbox{ plane } R_{a} =', Ra
print r'%\mbox{Rotor in }\bm{e}_{m}\bm{e}_{b}\mbox{ plane } R_{b} =', Rb
Rab = Ra * Rb # Compound Rotor
"""
Show that compound rotor is scalar plus bivector
"""
print r'%R_{a}R_{b} = S+\bm{B} =', Rab
Rab2 = Rab.get_grade(2)
print r'%\bm{B} =', Rab2
Rab2sq = Rab2 * Rab2 # Square of compound rotor bivector part
Ssq = (Rab.scalar())**2 # Square of compound rotor scalar part
Bsq = Rab2sq.scalar()
print r'%S^{2} =', Ssq
print r'%\bm{B}^{2} =', Bsq
Dsq = (Ssq - Bsq).expand().simplify()
print '%S^{2}-B^{2} =', Dsq
Dsq = Dsq.subs(nm**2, S(1)) # (e_m)**4 = 1
print '%S^{2}-B^{2} =', Dsq
Cases = [S(-1), S(1)] # -1/+1 squares for each basis vector
print r'#Consider all combinations of $\bm{e}_{a}^{2}$, $\bm{e}_{b}^{2}$'+\
r' and $\bm{e}_{m}^2$:'
for Na in Cases:
for Nb in Cases:
for Nm in Cases:
Ba_sq = -Na * Nm
Bb_sq = -Nb * Nm
if Ba_sq < 0:
Ca_th = cos(th_a)
Sa_th = sin(th_a)
else:
Ca_th = cosh(th_a)
Sa_th = sinh(th_a)
if Bb_sq < 0:
Cb_th = cos(th_b)
Sb_th = sin(th_b)
else:
Cb_th = cosh(th_b)
Sb_th = sinh(th_b)
print r'%\left [ \bm{e}_{a}^{2},\bm{e}_{b}^{2},\bm{e}_{m}^2\right ] =',\
[Na,Nb,Nm]
Dsq_tmp = Dsq.subs({
ca: Ca_th,
sa: Sa_th,
cb: Cb_th,
sb: Sb_th,
na: Na,
nb: Nb,
nm: Nm
})
print r'%S^{2}-\bm{B}^{2} =', Dsq_tmp, ' =', trigsimp(Dsq_tmp)
print r'#Thus we have shown that $R_{a}R_{b} = S+\bm{D} = e^{\bm{C}}$ where $\bm{C}$'+\
r' is a bivector blade.'
return
def test_mellin_transform_bessel():
from sympy import Max
MT = mellin_transform
# 8.4.19
assert MT(besselj(a, 2*sqrt(x)), x, s) == \
(gamma(a/2 + s)/gamma(a/2 - s + 1), (-re(a)/2, S(3)/4), True)
assert MT(sin(sqrt(x))*besselj(a, sqrt(x)), x, s) == \
(2**a*gamma(-2*s + S(1)/2)*gamma(a/2 + s + S(1)/2)/(
gamma(-a/2 - s + 1)*gamma(a - 2*s + 1)), (
-re(a)/2 - S(1)/2, S(1)/4), True)
assert MT(cos(sqrt(x))*besselj(a, sqrt(x)), x, s) == \
(2**a*gamma(a/2 + s)*gamma(-2*s + S(1)/2)/(
gamma(-a/2 - s + S(1)/2)*gamma(a - 2*s + 1)), (
-re(a)/2, S(1)/4), True)
assert MT(besselj(a, sqrt(x))**2, x, s) == \
(gamma(a + s)*gamma(S(1)/2 - s)
/ (sqrt(pi)*gamma(1 - s)*gamma(1 + a - s)),
(-re(a), S(1)/2), True)
assert MT(besselj(a, sqrt(x))*besselj(-a, sqrt(x)), x, s) == \
(gamma(s)*gamma(S(1)/2 - s)
/ (sqrt(pi)*gamma(1 - a - s)*gamma(1 + a - s)),
(0, S(1)/2), True)
# NOTE: prudnikov gives the strip below as (1/2 - re(a), 1). As far as
# I can see this is wrong (since besselj(z) ~ 1/sqrt(z) for z large)
assert MT(besselj(a - 1, sqrt(x))*besselj(a, sqrt(x)), x, s) == \
(gamma(1 - s)*gamma(a + s - S(1)/2)
/ (sqrt(pi)*gamma(S(3)/2 - s)*gamma(a - s + S(1)/2)),
(S(1)/2 - re(a), S(1)/2), True)
assert MT(besselj(a, sqrt(x))*besselj(b, sqrt(x)), x, s) == \
(4**s*gamma(1 - 2*s)*gamma((a + b)/2 + s)
/ (gamma(1 - s + (b - a)/2)*gamma(1 - s + (a - b)/2)
*gamma( 1 - s + (a + b)/2)),
(-(re(a) + re(b))/2, S(1)/2), True)
assert MT(besselj(a, sqrt(x))**2 + besselj(-a, sqrt(x))**2, x, s)[1:] == \
((Max(re(a), -re(a)), S(1)/2), True)
# Section 8.4.20
assert MT(bessely(a, 2*sqrt(x)), x, s) == \
(-cos(pi*(a/2 - s))*gamma(s - a/2)*gamma(s + a/2)/pi,
(Max(-re(a)/2, re(a)/2), S(3)/4), True)
assert MT(sin(sqrt(x))*bessely(a, sqrt(x)), x, s) == \
(-4**s*sin(pi*(a/2 - s))*gamma(S(1)/2 - 2*s)
* gamma((1 - a)/2 + s)*gamma((1 + a)/2 + s)
/ (sqrt(pi)*gamma(1 - s - a/2)*gamma(1 - s + a/2)),
(Max(-(re(a) + 1)/2, (re(a) - 1)/2), S(1)/4), True)
assert MT(cos(sqrt(x))*bessely(a, sqrt(x)), x, s) == \
(-4**s*cos(pi*(a/2 - s))*gamma(s - a/2)*gamma(s + a/2)*gamma(S(1)/2 - 2*s)
/ (sqrt(pi)*gamma(S(1)/2 - s - a/2)*gamma(S(1)/2 - s + a/2)),
(Max(-re(a)/2, re(a)/2), S(1)/4), True)
assert MT(besselj(a, sqrt(x))*bessely(a, sqrt(x)), x, s) == \
(-cos(pi*s)*gamma(s)*gamma(a + s)*gamma(S(1)/2 - s)
/ (pi**S('3/2')*gamma(1 + a - s)),
(Max(-re(a), 0), S(1)/2), True)
assert MT(besselj(a, sqrt(x))*bessely(b, sqrt(x)), x, s) == \
(-4**s*cos(pi*(a/2 - b/2 + s))*gamma(1 - 2*s)
* gamma(a/2 - b/2 + s)*gamma(a/2 + b/2 + s)
/ (pi*gamma(a/2 - b/2 - s + 1)*gamma(a/2 + b/2 - s + 1)),
(Max((-re(a) + re(b))/2, (-re(a) - re(b))/2), S(1)/2), True)
# NOTE bessely(a, sqrt(x))**2 and bessely(a, sqrt(x))*bessely(b, sqrt(x))
# are a mess (no matter what way you look at it ...)
assert MT(bessely(a, sqrt(x))**2, x, s)[1:] == \
((Max(-re(a), 0, re(a)), S(1)/2), True)
# Section 8.4.22
# TODO we can't do any of these (delicate cancellation)
# Section 8.4.23
assert MT(besselk(a, 2*sqrt(x)), x, s) == \
(gamma(
s - a/2)*gamma(s + a/2)/2, (Max(-re(a)/2, re(a)/2), oo), True)
assert MT(
besselj(a, 2 * sqrt(2 * sqrt(x))) * besselk(a, 2 * sqrt(2 * sqrt(x))),
x, s) == (4**(-s) * gamma(2 * s) * gamma(a / 2 + s) /
(2 * gamma(a / 2 - s + 1)), (Max(0, -re(a) / 2), oo), True)
# TODO bessely(a, x)*besselk(a, x) is a mess
assert MT(besseli(a, sqrt(x))*besselk(a, sqrt(x)), x, s) == \
(gamma(s)*gamma(
a + s)*gamma(-s + S(1)/2)/(2*sqrt(pi)*gamma(a - s + 1)),
(Max(-re(a), 0), S(1)/2), True)
assert MT(besseli(b, sqrt(x))*besselk(a, sqrt(x)), x, s) == \
(2**(2*s - 1)*gamma(-2*s + 1)*gamma(-a/2 + b/2 + s)* \
gamma(a/2 + b/2 + s)/(gamma(-a/2 + b/2 - s + 1)* \
gamma(a/2 + b/2 - s + 1)), (Max(-re(a)/2 - re(b)/2, \
re(a)/2 - re(b)/2), S(1)/2), True)
# TODO products of besselk are a mess
mt = MT(exp(-x / 2) * besselk(a, x / 2), x, s)
mt0 = gammasimp((trigsimp(gammasimp(mt[0].expand(func=True)))))
assert mt0 == 2 * pi**(S(3) / 2) * cos(pi * s) * gamma(-s + S(1) / 2) / (
(cos(2 * pi * a) - cos(2 * pi * s)) * gamma(-a - s + 1) *
gamma(a - s + 1))
assert mt[1:] == ((Max(-re(a), re(a)), oo), True)
def test_non_central_inertia():
# This tests that the calculation of Fr* does not depend the point
# about which the inertia of a rigid body is defined. This test solves
# exercises 8.12, 8.17 from Kane 1985.
# Declare symbols
q1, q2, q3 = dynamicsymbols('q1:4')
q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
u1, u2, u3, u4, u5 = dynamicsymbols('u1:6')
u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta')
a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
Q1, Q2, Q3 = symbols('Q1, Q2 Q3')
IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
# Reference Frames
F = ReferenceFrame('F')
P = F.orientnew('P', 'axis', [-theta, F.y])
A = P.orientnew('A', 'axis', [q1, P.x])
A.set_ang_vel(F, u1 * A.x + u3 * A.z)
# define frames for wheels
B = A.orientnew('B', 'axis', [q2, A.z])
C = A.orientnew('C', 'axis', [q3, A.z])
B.set_ang_vel(A, u4 * A.z)
C.set_ang_vel(A, u5 * A.z)
# define points D, S*, Q on frame A and their velocities
pD = Point('D')
pD.set_vel(A, 0)
# u3 will not change v_D_F since wheels are still assumed to roll without slip.
pD.set_vel(F, u2 * A.y)
pS_star = pD.locatenew('S*', e * A.y)
pQ = pD.locatenew('Q', f * A.y - R * A.x)
for p in [pS_star, pQ]:
p.v2pt_theory(pD, F, A)
# masscenters of bodies A, B, C
pA_star = pD.locatenew('A*', a * A.y)
pB_star = pD.locatenew('B*', b * A.z)
pC_star = pD.locatenew('C*', -b * A.z)
for p in [pA_star, pB_star, pC_star]:
p.v2pt_theory(pD, F, A)
# points of B, C touching the plane P
pB_hat = pB_star.locatenew('B^', -R * A.x)
pC_hat = pC_star.locatenew('C^', -R * A.x)
pB_hat.v2pt_theory(pB_star, F, B)
pC_hat.v2pt_theory(pC_star, F, C)
# the velocities of B^, C^ are zero since B, C are assumed to roll without slip
kde = [q1d - u1, q2d - u4, q3d - u5]
vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]
# inertias of bodies A, B, C
# IA22, IA23, IA33 are not specified in the problem statement, but are
# necessary to define an inertia object. Although the values of
# IA22, IA23, IA33 are not known in terms of the variables given in the
# problem statement, they do not appear in the general inertia terms.
inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
inertia_B = inertia(B, K, K, J)
inertia_C = inertia(C, K, K, J)
# define the rigid bodies A, B, C
rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
km = KanesMethod(F,
q_ind=[q1, q2, q3],
u_ind=[u1, u2],
kd_eqs=kde,
u_dependent=[u4, u5],
velocity_constraints=vc,
u_auxiliary=[u3])
forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x + Q2 * A.y + Q3 * A.z)]
bodies = [rbA, rbB, rbC]
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
fr, fr_star = km.kanes_equations(forces, bodies)
vc_map = solve(vc, [u4, u5])
# KanesMethod returns the negative of Fr, Fr* as defined in Kane1985.
fr_star_expected = Matrix([
-(IA + 2 * J * b**2 / R**2 + 2 * K + mA * a**2 + 2 * mB * b**2) *
u1.diff(t) - mA * a * u1 * u2,
-(mA + 2 * mB + 2 * J / R**2) * u2.diff(t) + mA * a * u1**2, 0
])
t = trigsimp(fr_star.subs(vc_map).subs({u3: 0})).doit().expand()
assert ((fr_star_expected - t).expand() == zeros(3, 1))
# define inertias of rigid bodies A, B, C about point D
# I_S/O = I_S/S* + I_S*/O
bodies2 = []
for rb, I_star in zip([rbA, rbB, rbC], [inertia_A, inertia_B, inertia_C]):
I = I_star + inertia_of_point_mass(rb.mass, rb.masscenter.pos_from(pD),
rb.frame)
bodies2.append(RigidBody('', rb.masscenter, rb.frame, rb.mass,
(I, pD)))
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
fr2, fr_star2 = km.kanes_equations(forces, bodies2)
t = trigsimp(fr_star2.subs(vc_map).subs({u3: 0})).doit()
assert (fr_star_expected - t).expand() == zeros(3, 1)
def test_trigsimp_issue_3826():
assert trigsimp(tan(2 * x).expand(trig=True)) == tan(2 * x)
def test_issue1274():
x = Symbol("x")
assert abs(trigsimp(2.0 * sin(x)**2 + 2.0 * cos(x)**2) - 2.0) < 1e-10
def test_I10():
assert trigsimp(
(tan(x)**2 + 1 - cos(x)**-2) / (sin(x)**2 + cos(x)**2 - 1)) == nan
T = ((M + J_ball/R**2)*d_r_t**2 + (J + M*r_t**2 + J_ball)*d_theta_t**2)/2
# potential energy
V = M*G*r_t*sin(theta_t)
# lagrange function
L = T - V
# replace functions through symbols
r_s , d_r_s , dd_r_s , theta_s, d_theta_s , dd_theta_s = sp.symbols('r_s , d_r_s , dd_r_s , theta_s, d_theta_s , dd_theta_s')
subs_list = [(dd_r_t,dd_r_s),(dd_theta_t,dd_theta_s),(d_r_t,d_r_s),(d_theta_t,d_theta_s),(r_t,r_s),(theta_t,theta_s)]
L = L.subs(subs_list)
# simplify L
L = L.expand()
L = sp.trigsimp(L)
# assitant terms
d_L_r = L.diff(r_s)
d_L_theta = L.diff(theta_s)
d_L_d_r = L.diff(d_r_s)
d_L_d_theta = L.diff(d_theta_s)
# replace symbols through functions
subs_list_rev = []
for tup in subs_list:
subs_list_rev.append((tup[1],tup[0]))
#d_L_r = d_L_r.subs(subs_list_rev)
#d_L_theta = d_L_theta.subs(subs_list_rev)
def test_issue_5948():
a, x, y = symbols('a x y')
assert trigsimp(diff(integrate(cos(x)/sin(x)**7, x), x)) == \
cos(x)/sin(x)**7
def eval_default_trig(*args):
return sympy.trigsimp(eval_default(*args))
def test_hyperbolic_simp():
x, y = symbols('x,y')
assert trigsimp(sinh(x)**2 + 1) == cosh(x)**2
assert trigsimp(cosh(x)**2 - 1) == sinh(x)**2
assert trigsimp(cosh(x)**2 - sinh(x)**2) == 1
assert trigsimp(1 - tanh(x)**2) == 1 / cosh(x)**2
assert trigsimp(1 - 1 / cosh(x)**2) == tanh(x)**2
assert trigsimp(tanh(x)**2 + 1 / cosh(x)**2) == 1
assert trigsimp(coth(x)**2 - 1) == 1 / sinh(x)**2
assert trigsimp(1 / sinh(x)**2 + 1) == 1 / tanh(x)**2
assert trigsimp(coth(x)**2 - 1 / sinh(x)**2) == 1
assert trigsimp(5 * cosh(x)**2 - 5 * sinh(x)**2) == 5
assert trigsimp(5 * cosh(x / 2)**2 -
2 * sinh(x / 2)**2) == 3 * cosh(x) / 2 + S(7) / 2
assert trigsimp(sinh(x) / cosh(x)) == tanh(x)
assert trigsimp(tanh(x)) == trigsimp(sinh(x) / cosh(x))
assert trigsimp(cosh(x) / sinh(x)) == 1 / tanh(x)
assert trigsimp(2 * tanh(x) * cosh(x)) == 2 * sinh(x)
assert trigsimp(coth(x)**3 * sinh(x)**3) == cosh(x)**3
assert trigsimp(y * tanh(x)**2 / sinh(x)**2) == y / cosh(x)**2
assert trigsimp(coth(x) / cosh(x)) == 1 / sinh(x)
for a in (pi / 6 * I, pi / 4 * I, pi / 3 * I):
assert trigsimp(sinh(a) * cosh(x) + cosh(a) * sinh(x)) == sinh(x + a)
assert trigsimp(-sinh(a) * cosh(x) + cosh(a) * sinh(x)) == sinh(x - a)
e = 2 * cosh(x)**2 - 2 * sinh(x)**2
assert trigsimp(log(e)) == log(2)
assert trigsimp(cosh(x)**2 * cosh(y)**2 - cosh(x)**2 * sinh(y)**2 -
sinh(x)**2,
recursive=True) == 1
assert trigsimp(sinh(x)**2 * sinh(y)**2 - sinh(x)**2 * cosh(y)**2 +
cosh(x)**2,
recursive=True) == 1
assert abs(trigsimp(2.0 * cosh(x)**2 - 2.0 * sinh(x)**2) - 2.0) < 1e-10
assert trigsimp(sinh(x)**2 / cosh(x)**2) == tanh(x)**2
assert trigsimp(sinh(x)**3 / cosh(x)**3) == tanh(x)**3
assert trigsimp(sinh(x)**10 / cosh(x)**10) == tanh(x)**10
assert trigsimp(cosh(x)**3 / sinh(x)**3) == 1 / tanh(x)**3
assert trigsimp(cosh(x) / sinh(x)) == 1 / tanh(x)
assert trigsimp(cosh(x)**2 / sinh(x)**2) == 1 / tanh(x)**2
assert trigsimp(cosh(x)**10 / sinh(x)**10) == 1 / tanh(x)**10
assert trigsimp(x * cosh(x) * tanh(x)) == x * sinh(x)
assert trigsimp(-sinh(x) + cosh(x) * tanh(x)) == 0
assert tan(x) != 1 / cot(x) # cot doesn't auto-simplify
assert trigsimp(tan(x) - 1 / cot(x)) == 0
assert trigsimp(3 * tanh(x)**7 - 2 / coth(x)**7) == tanh(x)**7
def test_trigsimp_issue_2515():
x = Symbol('x')
assert trigsimp(x * cos(x) * tan(x)) == x * sin(x)
assert trigsimp(-sin(x) + cos(x) * tan(x)) == 0
def express(expr, frame, frame2=None, variables=False):
"""
Global function for 'express' functionality.
Re-expresses a Vector, scalar(sympyfiable) or Dyadic in given frame.
Refer to the local methods of Vector and Dyadic for details.
If 'variables' is True, then the coordinate variables (CoordinateSym
instances) of other frames present in the vector/scalar field or
dyadic expression are also substituted in terms of the base scalars of
this frame.
Parameters
==========
expr : Vector/Dyadic/scalar(sympyfiable)
The expression to re-express in ReferenceFrame 'frame'
frame: ReferenceFrame
The reference frame to express expr in
frame2 : ReferenceFrame
The other frame required for re-expression(only for Dyadic expr)
variables : boolean
Specifies whether to substitute the coordinate variables present
in expr, in terms of those of frame
Examples
========
>>> from sympy.physics.vector import ReferenceFrame, outer, dynamicsymbols
>>> N = ReferenceFrame('N')
>>> q = dynamicsymbols('q')
>>> B = N.orientnew('B', 'Axis', [q, N.z])
>>> d = outer(N.x, N.x)
>>> from sympy.physics.vector import express
>>> express(d, B, N)
cos(q)*(B.x|N.x) - sin(q)*(B.y|N.x)
>>> express(B.x, N)
cos(q)*N.x + sin(q)*N.y
>>> express(N[0], B, variables=True)
B_x*cos(q(t)) - B_y*sin(q(t))
"""
_check_frame(frame)
if expr == 0:
return expr
if isinstance(expr, Vector):
#Given expr is a Vector
if variables:
#If variables attribute is True, substitute
#the coordinate variables in the Vector
frame_list = [x[-1] for x in expr.args]
subs_dict = {}
for f in frame_list:
subs_dict.update(f.variable_map(frame))
expr = expr.subs(subs_dict)
#Re-express in this frame
outvec = Vector([])
for i, v in enumerate(expr.args):
if v[1] != frame:
temp = frame.dcm(v[1]) * v[0]
if Vector.simp:
temp = temp.applyfunc(lambda x: trigsimp(x, method='fu'))
outvec += Vector([(temp, frame)])
else:
outvec += Vector([v])
return outvec
if isinstance(expr, Dyadic):
if frame2 is None:
frame2 = frame
_check_frame(frame2)
ol = Dyadic(0)
for i, v in enumerate(expr.args):
ol += express(v[0], frame, variables=variables) * \
(express(v[1], frame, variables=variables) |
express(v[2], frame2, variables=variables))
return ol
else:
if variables:
#Given expr is a scalar field
frame_set = set([])
expr = sympify(expr)
#Subsitute all the coordinate variables
for x in expr.free_symbols:
if isinstance(x, CoordinateSym) and x.frame != frame:
frame_set.add(x.frame)
subs_dict = {}
for f in frame_set:
subs_dict.update(f.variable_map(frame))
return expr.subs(subs_dict)
return expr
def test_trigsimp2():
x, y = symbols('x,y')
assert trigsimp(cos(x)**2 * sin(y)**2 + cos(x)**2 * cos(y)**2 + sin(x)**2,
recursive=True) == 1
assert trigsimp(sin(x)**2 * sin(y)**2 + sin(x)**2 * cos(y)**2 + cos(x)**2,
recursive=True) == 1
def test_sub_qdot():
# This test solves exercises 8.12, 8.17 from Kane 1985 and defines
# some velocities in terms of q, qdot.
## --- Declare symbols ---
q1, q2, q3 = dynamicsymbols('q1:4')
q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
u1, u2, u3 = dynamicsymbols('u1:4')
u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta')
a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
Q1, Q2, Q3 = symbols('Q1 Q2 Q3')
# --- Reference Frames ---
F = ReferenceFrame('F')
P = F.orientnew('P', 'axis', [-theta, F.y])
A = P.orientnew('A', 'axis', [q1, P.x])
A.set_ang_vel(F, u1 * A.x + u3 * A.z)
# define frames for wheels
B = A.orientnew('B', 'axis', [q2, A.z])
C = A.orientnew('C', 'axis', [q3, A.z])
## --- define points D, S*, Q on frame A and their velocities ---
pD = Point('D')
pD.set_vel(A, 0)
# u3 will not change v_D_F since wheels are still assumed to roll w/o slip
pD.set_vel(F, u2 * A.y)
pS_star = pD.locatenew('S*', e * A.y)
pQ = pD.locatenew('Q', f * A.y - R * A.x)
# masscenters of bodies A, B, C
pA_star = pD.locatenew('A*', a * A.y)
pB_star = pD.locatenew('B*', b * A.z)
pC_star = pD.locatenew('C*', -b * A.z)
for p in [pS_star, pQ, pA_star, pB_star, pC_star]:
p.v2pt_theory(pD, F, A)
# points of B, C touching the plane P
pB_hat = pB_star.locatenew('B^', -R * A.x)
pC_hat = pC_star.locatenew('C^', -R * A.x)
pB_hat.v2pt_theory(pB_star, F, B)
pC_hat.v2pt_theory(pC_star, F, C)
# --- relate qdot, u ---
# the velocities of B^, C^ are zero since B, C are assumed to roll w/o slip
kde = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]
kde += [u1 - q1d]
kde_map = solve(kde, [q1d, q2d, q3d])
for k, v in list(kde_map.items()):
kde_map[k.diff(t)] = v.diff(t)
# inertias of bodies A, B, C
# IA22, IA23, IA33 are not specified in the problem statement, but are
# necessary to define an inertia object. Although the values of
# IA22, IA23, IA33 are not known in terms of the variables given in the
# problem statement, they do not appear in the general inertia terms.
inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
inertia_B = inertia(B, K, K, J)
inertia_C = inertia(C, K, K, J)
# define the rigid bodies A, B, C
rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
## --- use kanes method ---
km = KanesMethod(F, [q1, q2, q3], [u1, u2], kd_eqs=kde, u_auxiliary=[u3])
forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x + Q2 * A.y + Q3 * A.z)]
bodies = [rbA, rbB, rbC]
# Q2 = -u_prime * u2 * Q1 / sqrt(u2**2 + f**2 * u1**2)
# -u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2) = R / Q1 * Q2
fr_expected = Matrix([
f * Q3 + M * g * e * sin(theta) * cos(q1),
Q2 + M * g * sin(theta) * sin(q1),
e * M * g * cos(theta) - Q1 * f - Q2 * R
])
#Q1 * (f - u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2)))])
fr_star_expected = Matrix([
-(IA + 2 * J * b**2 / R**2 + 2 * K + mA * a**2 + 2 * mB * b**2) *
u1.diff(t) - mA * a * u1 * u2,
-(mA + 2 * mB + 2 * J / R**2) * u2.diff(t) + mA * a * u1**2, 0
])
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
fr, fr_star = km.kanes_equations(forces, bodies)
assert (fr.expand() == fr_expected.expand())
assert ((fr_star_expected - trigsimp(fr_star)).expand() == zeros(3, 1))
def __init__(self, x, coords, debug=False, I=None):
"""
coords: list of coordinate variables
x: vector fuction of coordinate variables (parametric surface)
"""
self.I = I
self.x = x
self.coords = coords
self.basis = []
self.basis_str = []
self.embedded_basis = []
for u in coords:
tv = x.diff(u)
self.basis.append(tv)
(coefs, bases) = linear_expand(tv.obj)
tc = {}
for (coef, base) in zip(coefs, bases):
str_base = str(base)
tc[str_base] = coef
if str_base not in self.embedded_basis:
self.embedded_basis.append(str_base)
self.basis_str.append(tc)
self.gij = []
for base1 in self.basis:
tmp = []
for base2 in self.basis:
tmp.append(simplify(trigsimp((base1 | base2).scalar())))
self.gij.append(tmp)
for tv in self.basis_str:
for base in self.embedded_basis:
if base not in tv:
tv[base] = 0
self.dim = len(self.basis)
indexes = tuple(range(self.dim))
self.index = [()]
for i in indexes:
self.index.append(tuple(combinations(indexes, i + 1)))
self.index = tuple(self.index)
self.MFbasis = [[MV.ONE], self.basis]
for igrade in self.index[2:]:
grade = []
for iblade in igrade:
blade = MV(1, 'scalar')
for ibasis in iblade:
blade ^= self.basis[ibasis]
blade = blade.trigsimp(deep=True, recursive=True)
grade.append(blade)
self.MFbasis.append(grade)
self.E = self.MFbasis[-1][0]
self.E_sq = trigsimp((self.E * self.E).scalar(),
deep=True,
recursive=True)
duals = copy.copy(self.MFbasis[-2])
duals.reverse()
sgn = 1
self.rbasis = []
for dual in duals:
recpv = (sgn * dual * self.E).trigsimp(deep=True, recursive=True)
self.rbasis.append(recpv)
sgn = -sgn
self.dbasis = []
for base in self.basis:
dbase = []
for coord in self.coords:
d = base.diff(coord).trigsimp(deep=True, recursive=True)
dbase.append(d)
self.dbasis.append(dbase)
self.surface = {}
(coefs, bases) = linear_expand(self.x.obj)
for (coef, base) in zip(coefs, bases):
self.surface[str(base)] = coef
self.grad = MV()
self.grad.is_grad = True
self.grad.blade_rep = True
self.grad.igrade = 1
self.grad.rcpr_bases_MV = []
for rbase in self.rbasis:
self.grad.rcpr_bases_MV.append(rbase / self.E_sq)
self.grad.rcpr_bases_MV = tuple(self.grad.rcpr_bases_MV)
self.grad.coords = self.coords
self.grad.norm = self.E_sq
self.grad.connection = {}
if debug:
oprint('x', self.x, 'coords', self.coords, 'basis vectors',
self.basis, 'index', self.index, 'basis blades',
self.MFbasis, 'E', self.E, 'E**2', self.E_sq, '*basis',
duals, 'rbasis', self.rbasis, 'basis derivatives',
self.dbasis, 'surface', self.surface, 'basis strings',
self.basis_str, 'embedding basis', self.embedded_basis,
'metric tensor', self.gij)
