def two_wheel_3d_model(x, u, dt):
"""Two wheel 3D motion model
Parameters
----------
x :
u :
dt :
Returns
-------
"""
g1 = x[0, 0] + u[0] * cos(x[3, 0]) * dt
g2 = x[1, 0] + u[0] * sin(x[3, 0]) * dt
g3 = x[2, 0] + u[1] * dt
g4 = x[3, 0] + u[2] * dt
return np.array([g1, g2, g3, g4])
python类sin()的实例源码
def two_wheel_3d_deriv():
""" """
x1, x2, x3, x4, x5, x6, x7 = sympy.symbols("x1,x2,x3,x4,x5,x6,x7")
dt = sympy.symbols("dt")
# x1 - x
# x2 - y
# x3 - z
# x4 - theta
# x5 - v
# x6 - omega
# x7 - vz
# x, y, z, theta, v, omega, vz
f1 = x1 + x5 * sympy.cos(x4) * dt
f2 = x2 + x5 * sympy.sin(x4) * dt
f3 = x3 + x7 * dt
f4 = x4 + x6 * dt
f5 = x5
f6 = x6
f7 = x7
F = sympy.Matrix([f1, f2, f3, f4, f5, f6, f7])
pprint(F.jacobian([x1, x2, x3, x4, x5, x6, x7]))
def linearity_index_inverse_depth():
"""Linearity index of Inverse Depth Parameterization"""
D, rho, rho_0, d_1, sigma_rho = sympy.symbols("D,rho,rho_0,d_1,sigma_rho")
alpha = sympy.symbols("alpha")
u = (rho * sympy.sin(alpha)) / (rho_0 * d_1 * (rho_0 - rho) + rho * sympy.cos(alpha)) # NOQA
# first order derivative of u
u_p = sympy.diff(u, rho)
u_p = sympy.simplify(u_p)
# second order derivative of u
u_pp = sympy.diff(u_p, rho)
u_pp = sympy.simplify(u_pp)
# Linearity index
L = (u_pp * 2 * sigma_rho) / (u_p)
L = sympy.simplify(L)
print()
print("u: ", u)
print("u': ", u_p)
print("u'': ", u_pp)
# print("L = ", L)
print("L = ", L.subs(rho, 0))
print()
def test_epath_apply():
expr = [((x, 1, t), 2), ((3, y, 4), z)]
func = lambda expr: expr**2
assert epath("/*", expr, list) == [[(x, 1, t), 2], [(3, y, 4), z]]
assert epath("/*/[0]", expr, list) == [([x, 1, t], 2), ([3, y, 4], z)]
assert epath("/*/[1]", expr, func) == [((x, 1, t), 4), ((3, y, 4), z**2)]
assert epath("/*/[2]", expr, list) == expr
assert epath("/*/[0]/int", expr, func) == [((x, 1, t), 2), ((9, y, 16), z)]
assert epath("/*/[0]/Symbol", expr, func) == [((x**2, 1, t**2), 2),
((3, y**2, 4), z)]
assert epath(
"/*/[0]/int[1:]", expr, func) == [((x, 1, t), 2), ((3, y, 16), z)]
assert epath("/*/[0]/Symbol[1:]", expr, func) == [((x, 1, t**2),
2), ((3, y**2, 4), z)]
assert epath("/Symbol", x + y + z + 1, func) == x**2 + y**2 + z**2 + 1
assert epath("/*/*/Symbol", t + sin(x + 1) + cos(x + y + E), func) == \
t + sin(x**2 + 1) + cos(x**2 + y**2 + E)
def test_call():
x, y = symbols('x y')
# See the long history of this in issues 5026 and 5105.
raises(TypeError, lambda: sin(x)({ x : 1, sin(x) : 2}))
raises(TypeError, lambda: sin(x)(1))
# No effect as there are no callables
assert sin(x).rcall(1) == sin(x)
assert (1 + sin(x)).rcall(1) == 1 + sin(x)
# Effect in the pressence of callables
l = Lambda(x, 2*x)
assert (l + x).rcall(y) == 2*y + x
assert (x**l).rcall(2) == x**4
# TODO UndefinedFunction does not subclass Expr
#f = Function('f')
#assert (2*f)(x) == 2*f(x)
assert (Q.real & Q.positive).rcall(x) == Q.real(x) & Q.positive(x)
def test_hypersum():
from sympy import sin
assert simplify(summation(x**n/fac(n), (n, 1, oo))) == -1 + exp(x)
assert summation((-1)**n * x**(2*n) / fac(2*n), (n, 0, oo)) == cos(x)
assert simplify(summation((-1)**n*x**(2*n + 1) /
factorial(2*n + 1), (n, 3, oo))) == -x + sin(x) + x**3/6 - x**5/120
assert summation(1/(n + 2)**3, (n, 1, oo)) == -S(9)/8 + zeta(3)
assert summation(1/n**4, (n, 1, oo)) == pi**4/90
s = summation(x**n*n, (n, -oo, 0))
assert s.is_Piecewise
assert s.args[0].args[0] == -1/(x*(1 - 1/x)**2)
assert s.args[0].args[1] == (abs(1/x) < 1)
m = Symbol('n', integer=True, positive=True)
assert summation(binomial(m, k), (k, 0, m)) == 2**m
def test_cross_different_frames():
assert cross(N.x, A.x) == sin(q1)*A.z
assert cross(N.x, A.y) == cos(q1)*A.z
assert cross(N.x, A.z) == -sin(q1)*A.x - cos(q1)*A.y
assert cross(N.y, A.x) == -cos(q1)*A.z
assert cross(N.y, A.y) == sin(q1)*A.z
assert cross(N.y, A.z) == cos(q1)*A.x - sin(q1)*A.y
assert cross(N.z, A.x) == A.y
assert cross(N.z, A.y) == -A.x
assert cross(N.z, A.z) == 0
assert cross(N.x, A.x) == sin(q1)*A.z
assert cross(N.x, A.y) == cos(q1)*A.z
assert cross(N.x, A.x + A.y) == sin(q1)*A.z + cos(q1)*A.z
assert cross(A.x + A.y, N.x) == -sin(q1)*A.z - cos(q1)*A.z
assert cross(A.x, C.x) == sin(q3)*C.y
assert cross(A.x, C.y) == -sin(q3)*C.x + cos(q3)*C.z
assert cross(A.x, C.z) == -cos(q3)*C.y
assert cross(C.x, A.x) == -sin(q3)*C.y
assert cross(C.y, A.x) == sin(q3)*C.x - cos(q3)*C.z
assert cross(C.z, A.x) == cos(q3)*C.y
def __init__(self, n, index):
self.dim = n
if index == 2:
self.degree = 2
r = sqrt(3) / 6
data = [
(1.0, numpy.array([numpy.full(n, 2*r)])),
(+r, _s(n, -1, r)),
(-r, _s(n, +1, r)),
]
else:
assert index == 3
self.degree = 3
n2 = n // 2 if n % 2 == 0 else (n-1)//2
i_range = range(1, 2*n+1)
pts = [[
[sqrt(fr(2, 3)) * cos((2*k-1)*i*pi / n) for i in i_range],
[sqrt(fr(2, 3)) * sin((2*k-1)*i*pi / n) for i in i_range],
] for k in range(1, n2+1)]
if n % 2 == 1:
sqrt3pm = numpy.full(2*n, 1/sqrt(3))
sqrt3pm[1::2] *= -1
pts.append(sqrt3pm)
pts = numpy.vstack(pts).T
data = [(fr(1, 2*n), pts)]
self.points, self.weights = untangle(data)
reference_volume = 2**n
self.weights *= reference_volume
return
def __init__(self, n):
self.weights = numpy.full(n, 2*sympy.pi/n)
self.points = numpy.column_stack([
[sympy.cos(sympy.pi * sympy.Rational(2*k, n)) for k in range(n)],
[sympy.sin(sympy.pi * sympy.Rational(2*k, n)) for k in range(n)],
])
self.degree = n - 1
return
def _gen4_1():
pts = 2 * sqrt(5) * numpy.array([
[cos(2*i*pi/5) for i in range(5)],
[sin(2*i*pi/5) for i in range(5)],
]).T
data = [
(fr(7, 10), numpy.array([[0, 0]])),
(fr(3, 50), pts),
]
return 4, data
# The boolean tells whether the factor 2*pi is already in the weights
def __init__(self, settings):
settings.update(output_dim=4)
super().__init__(settings)
params = sp.symbols('x1, x2, x3, x4, tau')
x1, x2, x3, x4, tau = params
x = [x1, x2, x3, x4]
h = sp.Matrix([[x1]])
f = sp.Matrix([[x2],
[st.B * x1 * x4 ** 2 - st.B * st.G * sin(x3)],
[x4],
[(tau - 2 * st.M * x1 * x2 * x4
- st.M * st.G * x1 * cos(x3)) / (
st.M * x1 ** 2 + st.J + st.Jb)]])
q = sp.Matrix(pm.lie_derivatives(h, f, x, len(x) - 1))
dq = q.jacobian(x)
if dq.rank() != len(x):
raise Exception("System might not be observable")
# gets p = [p0, p1, ... pn-1]
p = pm.char_coefficients(self._settings["poles"])
k = p[::-1]
l = dq.inv() @ k
mat2array = [{'ImmutableMatrix': np.array}, 'numpy']
self.h_func = sp.lambdify((x1, x2, x3, x4, tau), h, modules=mat2array)
self.l_func = sp.lambdify((x1, x2, x3, x4, tau), l, modules=mat2array)
self.f_func = sp.lambdify((x1, x2, x3, x4, tau), f, modules=mat2array)
self.output = np.array(self._settings["initial state"], dtype=float)
def setUp(self):
self._x1, self._x2 = sp.symbols("x y")
self.x = sp.Matrix([self._x1, self._x2])
self.f = sp.Matrix([-self._x2**2, sp.sin(self._x1)])
self.h = sp.Matrix([self._x1**2 - sp.sin(self._x2)])
def __init__(self, settings):
Trajectory.__init__(self, settings)
# calculate symbolic derivatives up to order n
t, a, f, off, p = sp.symbols("t, a, f, off, p")
self.yd_sym = []
harmonic = a * sp.sin(2 * sp.pi * f * t + p) + off
for idx in range(settings["differential_order"] + 1):
self.yd_sym.append(harmonic.diff(t, idx))
# lambdify
for idx, val in enumerate(self.yd_sym):
self.yd_sym[idx] = sp.lambdify((t, a, f, off, p), val, "numpy")
def test_sin():
e = sin(x).lseries(x)
assert next(e) == x
assert next(e) == -x**3/6
assert next(e) == x**5/120
def test_issue_5183():
s = (x + 1/x).lseries()
assert [si for si in s] == [1/x, x]
assert next((x + x**2).lseries()) == x
assert next(((1 + x)**7).lseries(x)) == 1
assert next((sin(x + y)).series(x, n=3).lseries(y)) == x
# it would be nice if all terms were grouped, but in the
# following case that would mean that all the terms would have
# to be known since, for example, every term has a constant in it.
s = ((1 + x)**7).series(x, 1, n=None)
assert [next(s) for i in range(2)] == [128, -448 + 448*x]
def test_finite_diff():
assert finite_diff(x**2 + 2*x + 1, x) == 2*x + 3
assert finite_diff(y**3 + 2*y**2 + 3*y + 5, y) == 3*y**2 + 7*y + 6
assert finite_diff(z**2 - 2*z + 3, z) == 2*z - 1
assert finite_diff(w**2 + 3*w - 2, w) == 2*w + 4
assert finite_diff(sin(x), x, pi/6) == -sin(x) + sin(x + pi/6)
assert finite_diff(cos(y), y, pi/3) == -cos(y) + cos(y + pi/3)
assert finite_diff(x**2 - 2*x + 3, x, 2) == 4*x
assert finite_diff(n**2 - 2*n + 3, n, 3) == 6*n + 3
def test_functions():
assert residue(1/sin(x), x, 0) == 1
assert residue(2/sin(x), x, 0) == 2
assert residue(1/sin(x)**2, x, 0) == 0
assert residue(1/sin(x)**5, x, 0) == S(3)/8
def test_epath_select():
expr = [((x, 1, t), 2), ((3, y, 4), z)]
assert epath("/*", expr) == [((x, 1, t), 2), ((3, y, 4), z)]
assert epath("/*/*", expr) == [(x, 1, t), 2, (3, y, 4), z]
assert epath("/*/*/*", expr) == [x, 1, t, 3, y, 4]
assert epath("/*/*/*/*", expr) == []
assert epath("/[:]", expr) == [((x, 1, t), 2), ((3, y, 4), z)]
assert epath("/[:]/[:]", expr) == [(x, 1, t), 2, (3, y, 4), z]
assert epath("/[:]/[:]/[:]", expr) == [x, 1, t, 3, y, 4]
assert epath("/[:]/[:]/[:]/[:]", expr) == []
assert epath("/*/[:]", expr) == [(x, 1, t), 2, (3, y, 4), z]
assert epath("/*/[0]", expr) == [(x, 1, t), (3, y, 4)]
assert epath("/*/[1]", expr) == [2, z]
assert epath("/*/[2]", expr) == []
assert epath("/*/int", expr) == [2]
assert epath("/*/Symbol", expr) == [z]
assert epath("/*/tuple", expr) == [(x, 1, t), (3, y, 4)]
assert epath("/*/__iter__?", expr) == [(x, 1, t), (3, y, 4)]
assert epath("/*/int|tuple", expr) == [(x, 1, t), 2, (3, y, 4)]
assert epath("/*/Symbol|tuple", expr) == [(x, 1, t), (3, y, 4), z]
assert epath("/*/int|Symbol|tuple", expr) == [(x, 1, t), 2, (3, y, 4), z]
assert epath("/*/int|__iter__?", expr) == [(x, 1, t), 2, (3, y, 4)]
assert epath("/*/Symbol|__iter__?", expr) == [(x, 1, t), (3, y, 4), z]
assert epath(
"/*/int|Symbol|__iter__?", expr) == [(x, 1, t), 2, (3, y, 4), z]
assert epath("/*/[0]/int", expr) == [1, 3, 4]
assert epath("/*/[0]/Symbol", expr) == [x, t, y]
assert epath("/*/[0]/int[1:]", expr) == [1, 4]
assert epath("/*/[0]/Symbol[1:]", expr) == [t, y]
assert epath("/Symbol", x + y + z + 1) == [x, y, z]
assert epath("/*/*/Symbol", t + sin(x + 1) + cos(x + y + E)) == [x, x, y]
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) == 1/tan(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) == 3*cos(x)/2 + S(7)/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(sin(x + y) + sin(x - y)) == 2*sin(x)*cos(y)
assert trigsimp(sin(x + y) - sin(x - y)) == 2*sin(y)*cos(x)
assert trigsimp(cos(x + y) + cos(x - y)) == 2*cos(x)*cos(y)
assert trigsimp(cos(x + y) - cos(x - y)) == -2*sin(x)*sin(y)
assert ratsimp(trigsimp(tan(x + y) - tan(x)/(1 - tan(x)*tan(y)))) == \
sin(y)/(-sin(y)*tan(x) + cos(y)) # -tan(y)/(tan(x)*tan(y) - 1)
assert trigsimp(sinh(x + y) + sinh(x - y)) == 2*sinh(x)*cosh(y)
assert trigsimp(sinh(x + y) - sinh(x - y)) == 2*sinh(y)*cosh(x)
assert trigsimp(cosh(x + y) + cosh(x - y)) == 2*cosh(x)*cosh(y)
assert trigsimp(cosh(x + y) - cosh(x - y)) == 2*sinh(x)*sinh(y)
assert ratsimp(trigsimp(tanh(x + y) - tanh(x)/(1 + tanh(x)*tanh(y)))) == \
sinh(y)/(sinh(y)*tanh(x) + cosh(y))
assert trigsimp(cos(0.12345)**2 + sin(0.12345)**2) == 1
e = 2*sin(x)**2 + 2*cos(x)**2
assert trigsimp(log(e)) == log(2)
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)
