def _dgm_right(robo, symo, i, j, trig_subs=True, sep_const=False):
k = robo.common_root(i, j)
chain1 = robo.chain(i, k)
chain2 = robo.chain(j, k)
chain2.reverse()
complete_chain = (chain1 + chain2 + [None])
T_out = {(i, i): eye(4)}
T_res = eye(4)
T = eye(4)
for indx, x in enumerate(complete_chain[:-1]):
inverted = indx < len(chain1)
T = T * _transform(robo, x, inverted)
if trig_subs:
for ang, name in robo.get_angles(x):
symo.trig_replace(T, ang, name)
T = T.expand()
T = T.applyfunc(symo.CS12_simp)
x_next = complete_chain[indx + 1]
if inverted:
t_name = (i, robo.ant[x])
else:
t_name = (i, x)
T_out[t_name] = T_res * T
if robo.paral(x, x_next):
continue
T_res = T_out[t_name]
T = eye(4)
return T_out
def _dgm_one(robo, symo, i, j, fast_form=True,
forced=False, trig_subs=True):
k = robo.common_root(i, j)
is_loop = i > robo.NL and j > robo.NL
chain1 = robo.chain(j, k)
chain2 = robo.chain(i, k)
chain2.reverse()
complete_chain = (chain1 + chain2 + [None])
T_res = eye(4)
T = eye(4)
for indx, x in enumerate(complete_chain[:-1]):
inverted = indx >= len(chain1)
T = _transform(robo, x, inverted) * T
if trig_subs:
for ang, name in robo.get_angles(x):
symo.trig_replace(T, ang, name)
T = T.expand()
T = T.applyfunc(symo.CS12_simp)
if is_loop:
T = T.applyfunc(symo.C2S2_simp)
x_next = complete_chain[indx + 1]
if robo.paral(x, x_next): # false if x_next is None
continue
T_res = T * T_res
T = eye(4)
if fast_form:
_dgm_rename(robo, symo, T_res, x, i, j, inverted, forced)
if not fast_form and forced:
_dgm_rename(robo, symo, T_res, x, i, j, inverted, forced)
return T_res
def rne_park_backward(rbtdef, geom, fw_results, ifunc=None):
'''RNE backward pass.'''
V, dV = fw_results
if not ifunc:
ifunc = identity
# extend Tdh_inv so that Tdh_inv[dof] return identity
Tdh_inv = geom.Tdh_inv + [eye(4)]
F = list(range(rbtdef.dof + 1))
F[rbtdef.dof] = zeros((6, 1))
tau = zeros((rbtdef.dof, 1))
fric = frictionforce(rbtdef)
Idrive = driveinertiaterm(rbtdef)
# Backward
for i in range(rbtdef.dof - 1, -1, -1):
Llm = (rbtdef.L[i].row_join(skew(rbtdef.l[i]))).col_join(
(-skew(rbtdef.l[i])).row_join(eye(3) * rbtdef.m[i]))
F[i] = Adjdual(Tdh_inv[i + 1], F[i + 1]) + \
Llm * dV[i] - adjdual(V[i], Llm * V[i])
F[i] = ifunc(F[i])
tau[i] = ifunc((geom.S[i].transpose() * F[i])[0] + fric[i] + Idrive[i])
return tau
def test_submatrix_assignment():
m = zeros(4)
m[2:4, 2:4] = eye(2)
assert m == Matrix(((0,0,0,0),
(0,0,0,0),
(0,0,1,0),
(0,0,0,1)))
m[0:2, 0:2] = eye(2)
assert m == eye(4)
m[:,0] = Matrix(4,1,(1,2,3,4))
assert m == Matrix(((1,0,0,0),
(2,1,0,0),
(3,0,1,0),
(4,0,0,1)))
m[:,:] = zeros(4)
assert m == zeros(4)
m[:,:] = ((1,2,3,4),(5,6,7,8),(9, 10, 11, 12),(13,14,15,16))
assert m == Matrix(((1,2,3,4),
(5,6,7,8),
(9, 10, 11, 12),
(13,14,15,16)))
m[0:2, 0] = [0,0]
assert m == Matrix(((0,2,3,4),
(0,6,7,8),
(9, 10, 11, 12),
(13,14,15,16)))
def rotation_from_vectors(v1, v2):
"""
Find the rotation Matrix R that fullfils:
R*v2 = v1
Jur van den Berg,
Calculate Rotation Matrix to align Vector A to Vector B in 3d?,
URL (version: 2016-09-01): https://math.stackexchange.com/q/476311
"""
v1 = sp.Matrix(v1).normalized()
v2 = sp.Matrix(v2).normalized()
ax = v1.cross(v2)
s = ax.norm()
c = v1.dot(v2)
if c==1:
return sp.eye(3)
if c==-1:
return -sp.eye(3)
u1, u2, u3 = ax
u_ = sp.Matrix((( 0, -u3, u2),
( u3, 0, -u1),
(-u2, u1, 0)))
R = sp.eye(3) - u_ + u_**2 * (1-c)/s**2
return R
def _form_coefficient_matrices(self):
"""Form the coefficient matrices C_0, C_1, and C_2."""
# Extract dimension variables
l, m, n, o, s, k = self._dims
# Build up the coefficient matrices C_0, C_1, and C_2
# If there are configuration constraints (l > 0), form C_0 as normal.
# If not, C_0 is I_(nxn). Note that this works even if n=0
if l > 0:
f_c_jac_q = self.f_c.jacobian(self.q)
self._C_0 = (eye(n) - self._Pqd * (f_c_jac_q *
self._Pqd).LUsolve(f_c_jac_q)) * self._Pqi
else:
self._C_0 = eye(n)
# If there are motion constraints (m > 0), form C_1 and C_2 as normal.
# If not, C_1 is 0, and C_2 is I_(oxo). Note that this works even if
# o = 0.
if m > 0:
f_v_jac_u = self.f_v.jacobian(self.u)
temp = f_v_jac_u * self._Pud
if n != 0:
f_v_jac_q = self.f_v.jacobian(self.q)
self._C_1 = -self._Pud * temp.LUsolve(f_v_jac_q)
else:
self._C_1 = 0
self._C_2 = (eye(o) - self._Pud *
temp.LUsolve(f_v_jac_u)) * self._Pui
else:
self._C_1 = 0
self._C_2 = eye(o)
def regular_completion(matrix):
m,n = matrix.shape
r = matrix.rank()
#~ IPS()
assert m!=n, "There is no regular completion of a square matrix."
if m<n:
assert r==m, "Matrix does not have full row rank."
A, B, V_pi = reshape_matrix_columns(matrix)
zeros = sp.zeros(n-m,m)
ones = sp.eye(n-m)
S = st.col_stack(zeros,ones)
completion = S*V_pi.T
regular_matrix = st.row_stack(matrix,completion)
assert st.rnd_number_rank(regular_matrix)==n, "Regular completion seems to be wrong."
elif n<m:
assert r==n, "Matrix does not have full column rank."
A, B, V_pi = reshape_matrix_columns(matrix.T)
zeros = sp.zeros(m-n,n)
ones = sp.eye(m-n)
S = st.col_stack(zeros,ones)
completion = V_pi*S.T
regular_matrix = st.col_stack(completion,matrix)
assert st.rnd_number_rank(regular_matrix)==m, "Regular completion seems to be wrong."
return completion
def compute_vect1_from_pixel(self):
# Parameters related to Mirror 1:
self.k1, self.c1 = symbols("k_1, c_1", real=True, positive=True)
self.u1, self.v1 = symbols("u_1, v_1", real=True, nonnegative=True)
# TODO: add assumption for k > 2
# Viewpoint (focus) position vector:
self.f1x, self.f1y, self.f1z = symbols("x_f_1, y_f_1, z_f_1", real=True)
# (Coaxial alignment assumption)
# where
self.f1x = 0
self.f1y = 0
self.f1z = self.c1
self.f1 = Matrix([self.f1x, self.f1y, self.f1z])
# Pixel vector
self.m1h = Matrix([self.u1, self.v1, 1])
# Point in normalized projection plane
self.q1 = self.Kc_inv * self.m1h
# Point in mirror wrt C
self.t1 = self.c1 / (self.k1 - self.q1.norm() * sqrt(self.k1 * (self.k1 - 2)))
self.p1 = self.t1 * self.q1
self.p1h = self.p1.col_join(eye(1))
# Transform matrix from C to F1 frame
self.T1_CtoF1 = eye(3).row_join(-self.f1)
# Direction vector
self.d1_vect = self.T1_CtoF1 * self.p1h
return self.d1_vect
def DHMaker(segments):
dh_matrices = {}
inverse_dh_matrices = {}
required_parameters = []
for segment in segments:
if segment.theta is not None:
theta = segment.theta
else:
theta = sp.Symbol(str('theta_' + segment.label))
required_parameters.append(theta)
if segment.alpha is not None:
alpha = segment.alpha
else:
alpha = sp.Symbol(str('alpha_' + segment.label))
required_parameters.append(alpha)
if segment.r is not None:
r = segment.r
else:
r = sp.Symbol(str('r_' + segment.label))
required_parameters.append(r)
if segment.d is not None:
d = segment.d
else:
d = sp.Symbol(str('d_' + segment.label))
required_parameters.append(d)
# Components of the dh matrix
rotation_matrix = sp.Matrix([[sp.cos(theta), -sp.sin(theta)*sp.cos(alpha), sp.sin(theta)*sp.sin(alpha)],
[sp.sin(theta), sp.cos(theta)*sp.cos(alpha), -sp.cos(theta)*sp.sin(alpha)],
[0, sp.sin(alpha), sp.cos(alpha)]])
inverse_rotation = rotation_matrix.T
translation_matrix = sp.Matrix([r*sp.cos(theta),r*sp.sin(theta),d])
inverse_translation = -inverse_rotation*translation_matrix
last_row = sp.Matrix([[0, 0, 0, 1]])
# Compose the forwards and inverse DH Matrices
dh_matrix = sp.Matrix.vstack(sp.Matrix.hstack(rotation_matrix, translation_matrix), last_row)
inverse_dh_matrix = sp.Matrix.vstack(sp.Matrix.hstack(inverse_rotation, inverse_translation), last_row)
inverse_dh_matrices[segment.label] = inverse_dh_matrix
dh_matrices[segment.label] = dh_matrix
# Finally, flatten all the matrices into end-to-end transformation matrices
compound_dh_matrix = sp.eye(4)
compound_inverse_dh_matrix = sp.eye(4)
for segment in segments:
compound_dh_matrix *= dh_matrices[segment.label]
for segment in reversed(segments):
compound_inverse_dh_matrix *= inverse_dh_matrices[segment.label]
# Chop off terms with small coefficients
compound_dh_matrix = compound_dh_matrix.applyfunc(coeff_chop)
compound_inverse_dh_matrix = compound_inverse_dh_matrix.applyfunc(coeff_chop)
actual_reqs = compound_dh_matrix.atoms(sp.Symbol).union(compound_inverse_dh_matrix.atoms(sp.Symbol))
required_parameters = [str(req) for req in required_parameters if req in actual_reqs] # This preserves the order in segments
return compound_dh_matrix, compound_inverse_dh_matrix, required_parameters
def __init__(self, symo=None, trig_subs=False, simplify=True):
self.rot = CompTransf(0, 0, 0)
self.rot_mat = eye(3)
self.trans = zeros(3, 1)
self.symo = symo
self.trig_subs = trig_subs and symo is not None
self.T_tmp = eye(4)
self.simplify = simplify
def lm(f_str, sym_str):
[f, symbols] = pr(f_str, sym_str)
# # of variables
D = len(symbols)
hess = sp.hessian(f, symbols)
mexpr = sp.Matrix([f])
grad = mexpr.jacobian(symbols)
grad = grad.T
# initial value
xk = sp.zeros(D, 1)
fk = 0
uk = 0.000000001
epsilon = 0.00001
for k in range(100):
Xk_map = {}
for i in range(D):
Xk_map[symbols[i]] = xk[i, 0]
fk = f.evalf(subs=Xk_map)
gk = grad.evalf(subs=Xk_map)
Gk = hess.evalf(subs=Xk_map)
if gk.norm() < epsilon:
break
while True:
eigvalue = sp.Matrix.eigenvals(Gk + uk * sp.eye(D))
if allzero(eigvalue):
break
else:
uk = uk * 4
# sk = sp.(A=Gk + uk * sp.ones(D), b=-gk)
sk = (Gk + uk * sp.eye(D)).LUsolve(-gk)
Xsk_map = {}
for i in range(D):
Xsk_map[symbols[i]] = xk[i, 0] + sk[i, 0]
fxsk = f.evalf(subs=Xsk_map)
delta_fk = fk - fxsk
t1 = (gk.T * sk)[0, 0]
t2 = (0.5 * sk.T * Gk * sk)[0, 0]
qk = fk + t1 + t2
delta_qk = fk - qk
rk = delta_fk / delta_qk
if rk < 0.25:
uk = uk * 4
elif rk > 0.75:
uk = uk / 2
else:
uk = uk
if rk <= 0:
xk = xk
else:
xk = xk + sk
print f, symbols
for i in range(D):
print symbols[i], ' = ', xk[i]
print 'min f =', fk
return [xk, fk, k]
def __init__(self, simplify=True):
self.simplify = simplify
self.constants = []
self.params = []
self.joints_sym = [SymbolicRevoluteJoint(0, 0, 0, 0)]
self.joints = [SymbolicRevoluteJoint(0, 0, 0, 0)]
self.hms_sym = [sympy.eye(4)]
self.hms = [sympy.eye(4)]
self.functional = False
def test_power():
A = Matrix([[2,3],[4,5]])
assert (A**5)[:] == [6140, 8097, 10796, 14237]
A = Matrix([[2, 1, 3],[4,2, 4], [6,12, 1]])
assert (A**3)[:] == [290, 262, 251, 448, 440, 368, 702, 954, 433]
assert A**0 == eye(3)
assert A**1 == A
assert (Matrix([[2]]) ** 100)[0,0] == 2**100
assert eye(2)**10000000 == eye(2)
def rotation_matrix(self, other):
"""
Returns the direction cosine matrix(DCM), also known as the
'rotation matrix' of this coordinate system with respect to
another system.
If v_a is a vector defined in system 'A' (in matrix format)
and v_b is the same vector defined in system 'B', then
v_a = A.rotation_matrix(B) * v_b.
A SymPy Matrix is returned.
Parameters
==========
other : CoordSysCartesian
The system which the DCM is generated to.
Examples
========
>>> from sympy.vector import CoordSysCartesian
>>> from sympy import symbols
>>> q1 = symbols('q1')
>>> N = CoordSysCartesian('N')
>>> A = N.orient_new_axis('A', q1, N.i)
>>> N.rotation_matrix(A)
Matrix([
[1, 0, 0],
[0, cos(q1), -sin(q1)],
[0, sin(q1), cos(q1)]])
"""
from sympy.vector.functions import _path
if not isinstance(other, CoordSysCartesian):
raise TypeError(str(other) +
" is not a CoordSysCartesian")
#Handle special cases
if other == self:
return eye(3)
elif other == self._parent:
return self._parent_rotation_matrix
elif other._parent == self:
return other._parent_rotation_matrix.T
#Else, use tree to calculate position
rootindex, path = _path(self, other)
result = eye(3)
i = -1
for i in range(rootindex):
result *= path[i]._parent_rotation_matrix
i += 2
while i < len(path):
result *= path[i]._parent_rotation_matrix.T
i += 1
return result
def test_can_transf_matrix():
dimsys = DimensionSystem((length, mass, time))
assert dimsys._can_transf_matrix is None
assert dimsys.can_transf_matrix == eye(3)
assert dimsys._can_transf_matrix == eye(3)
dimsys = DimensionSystem((length, velocity, action))
assert dimsys.can_transf_matrix == Matrix(((0, 1, 0), (1, 0, 1),
(0, -2, -1)))
def test_util():
v1 = Matrix(1,3,[1,2,3])
v2 = Matrix(1,3,[3,4,5])
assert v1.cross(v2) == Matrix(1,3,[-2,4,-2])
assert v1.norm() == sqrt(14)
# cofactor
assert eye(3) == eye(3).cofactorMatrix()
test = Matrix([[1,3,2],[2,6,3],[2,3,6]])
assert test.cofactorMatrix() == Matrix([[27,-6,-6],[-12,2,3],[-3,1,0]])
test = Matrix([[1,2,3],[4,5,6],[7,8,9]])
assert test.cofactorMatrix() == Matrix([[-3,6,-3],[6,-12,6],[-3,6,-3]])
def test_subs():
A = ImmutableMatrix([[1, 2], [3, 4]])
B = ImmutableMatrix([[1, 2], [x, 4]])
C = ImmutableMatrix([[-x, x*y], [-(x + y), y**2]])
assert B.subs(x, 3) == A
assert (x*B).subs(x, 3) == 3*A
assert (x*eye(2) + B).subs(x, 3) == 3*eye(2) + A
assert C.subs([[x, -1], [y, -2]]) == A
assert C.subs([(x, -1), (y, -2)]) == A
assert C.subs({x: -1, y: -2}) == A
assert C.subs({x: y - 1, y: x - 1}, simultaneous=True) == \
ImmutableMatrix([[1 - y, (x - 1)*(y - 1)], [2 - x - y, (x - 1)**2]])
def _generate_matrix(self):
'''Generate Matrixs for form homotopy equations'''
self.Lamda_matrix = sym.zeros(self.num_states)
self.Lamda_matrix[self.num_states/2:, self.num_states/2:] = self.parameters['homotopy_control']*sym.eye(self.num_states/2)
self.Inv_Lamda_matrix = sym.eye(self.num_states)
self.Inv_Lamda_matrix[self.num_states/2:, self.num_states/2:] = (1-self.parameters['homotopy_control'])*sym.eye(self.num_states/2)
self.k_matrix = self.parameters['tracking_dynamic_control'] * sym.eye(self.num_states)
return self
def test_QR_non_square():
A = Matrix([[9,0,26],[12,0,-7],[0,4,4],[0,-3,-3]])
Q, R = A.QRdecomposition()
assert Q.T * Q == eye(Q.cols)
assert R.is_upper()
assert A == Q*R
A = Matrix([[1,-1,4],[1,4,-2],[1,4,2],[1,-1,0]])
Q, R = A.QRdecomposition()
assert Q.T * Q == eye(Q.cols)
assert R.is_upper()
assert A == Q*R
def test_submatrix():
m0 = eye(4)
assert m0[0:3, 0:3] == eye(3)
assert m0[2:4, 0:2] == zeros(2)
m1 = Matrix(3,3, lambda i,j: i+j)
assert m1[0,:] == Matrix(1,3,(0,1,2))
assert m1[1:3, 1] == Matrix(2,1,(2,3))
m2 = Matrix([[0,1,2,3],[4,5,6,7],[8,9,10,11],[12,13,14,15]])
assert m2[:,-1] == Matrix(4,1,[3,7,11,15])
assert m2[-2:,:] == Matrix([[8,9,10,11],[12,13,14,15]])
def kPj(self, robo, antPj, antRj, k, chainj):
T = eye(4)
for i in chainj:
if i > k:
kTj = eye(4)
kTj[0:3, 0:3] = antRj[i]
kTj[0:3, 3:] = antPj[i]
T = kTj * T
Px = T[0, 3]
Py = T[1, 3]
Pz = T[2, 3]
return Px, Py, Pz
def test_inverse():
A = eye(4)
assert A.inv() == eye(4)
assert A.inv("LU") == eye(4)
assert A.inv("ADJ") == eye(4)
A = Matrix([[2,3,5],
[3,6,2],
[8,3,6]])
Ainv = A.inv()
assert A*Ainv == eye(3)
assert A.inv("LU") == Ainv
assert A.inv("ADJ") == Ainv
def test_issue_15601():
if not np:
skip("Numpy not installed")
M = MatrixSymbol("M", 3, 3)
N = MatrixSymbol("N", 3, 3)
expr = M*N
f = lambdify((M, N), expr, "numpy")
with warns_deprecated_sympy():
ans = f(eye(3), eye(3))
assert np.array_equal(ans, np.array([1, 0, 0, 0, 1, 0, 0, 0, 1]))
def test_QR():
A = Matrix([[1,2],[2,3]])
Q, S = A.QRdecomposition()
R = Rational
assert Q == Matrix([[5**R(-1,2), (R(2)/5)*(R(1)/5)**R(-1,2)], [2*5**R(-1,2), (-R(1)/5)*(R(1)/5)**R(-1,2)]])
assert S == Matrix([[5**R(1,2), 8*5**R(-1,2)], [0, (R(1)/5)**R(1,2)]])
assert Q*S == A
assert Q.T * Q == eye(2)
A = Matrix([[1,1,1],[1,1,3],[2,3,4]])
Q, R = A.QRdecomposition()
assert Q*Q.T == eye(Q.rows)
assert R.is_upper()
assert A == Q*R
def test_block_index():
I = Identity(3)
Z = ZeroMatrix(3, 3)
B = BlockMatrix([[I, I], [I, I]])
e3 = ImmutableMatrix(eye(3))
BB = BlockMatrix([[e3, e3], [e3, e3]])
assert B[0, 0] == B[3, 0] == B[0, 3] == B[3, 3] == 1
assert B[4, 3] == B[5, 1] == 0
BB = BlockMatrix([[e3, e3], [e3, e3]])
assert B.as_explicit() == BB.as_explicit()
BI = BlockMatrix([[I, Z], [Z, I]])
assert BI.as_explicit().equals(eye(6))
def test_power():
A = Matrix([[2,3],[4,5]])
assert (A**5)[:] == [6140, 8097, 10796, 14237]
A = Matrix([[2, 1, 3],[4,2, 4], [6,12, 1]])
assert (A**3)[:] == [290, 262, 251, 448, 440, 368, 702, 954, 433]
assert A**0 == eye(3)
assert A**1 == A
assert (Matrix([[2]]) ** 100)[0,0] == 2**100
assert eye(2)**10000000 == eye(2)
assert Matrix([[1, 2], [3, 4]])**Integer(2) == Matrix([[7, 10], [15, 22]])
A = Matrix([[33, 24], [48, 57]])
assert (A**(S(1)/2))[:] == [5, 2, 4, 7]
A = Matrix([[0, 4], [-1, 5]])
assert (A**(S(1)/2))**2 == A
def intern_test(np, nq):
N = np + nq
xx = st.symb_vector('x1:{0}'.format(N*2+1))
P0 = st.symbMatrix(np, N, s='A')
P1 = st.symbMatrix(np, N, s='B')
P2 = st.symbMatrix(np, N, s='C')
P0bar, P1bar = mt.transform_2nd_to_1st_order_matrices(P0, P1, P2, xx)
self.assertEqual(P0bar[:N, N:], sp.eye(N))
self.assertEqual(P1bar[:N, :N], -sp.eye(N))
self.assertEqual(P0bar[N:, :N], P0)
self.assertEqual(P1bar[N:, :], P1.row_join(P2))
def test_get_rot_matrices1():
B_A = Matrix([
[1, 0, 0],
[0, cos(q2), sin(q2)],
[0, -sin(q2), cos(q2)]
])
A_B = Matrix([
[1, 0, 0],
[0, cos(q2), -sin(q2)],
[0, sin(q2), cos(q2)]
])
B_C = Matrix([
[cos(q3), 0, sin(q3)],
[0, 1, 0],
[-sin(q3), 0, cos(q3)]
])
C_B = Matrix([
[cos(q3), 0, -sin(q3)],
[0, 1, 0],
[sin(q3), 0, cos(q3)]
])
assert B.get_rot_matrices(B) == [eye(3)]
assert B.get_rot_matrices(A) == [A_B]
assert A.get_rot_matrices(B) == [B_A]
assert A.get_rot_matrices(C) == [C_B, B_A]
assert C.get_rot_matrices(A) == [A_B, B_C]
def HT(R, Px=0.0, Py=0.0, Pz=0.0):
T = eye(4)
T[0:3,0:3] = R
T[0,3] = Px
T[1,3] = Py
T[2,3] = Pz
return T
def __init__(self, scheme):
# pylint: disable=unsubscriptable-object
self.nvtot = scheme.s.shape[0]
self.consm = list(scheme.consm.keys())
self.param = scheme.param
self.dim = scheme.dim
self.is_stable_l2 = True
if scheme.rel_vel is None:
jacobian = scheme.EQ.jacobian(self.consm)
else:
jacobian = (scheme.Tu * scheme.EQ).jacobian(self.consm)
relax_mat_m = sp.eye(self.nvtot) - sp.diag(*scheme.s)
relax_mat_m[:, :len(self.consm)] += sp.diag(*scheme.s) * jacobian
if scheme.rel_vel is not None:
relax_mat_m = scheme.Tmu * relax_mat_m * scheme.Tu
relax_mat_m = relax_mat_m.subs(
[
(i, j) for i, j in zip(
[rel_ux, rel_uy, rel_uz], scheme.rel_vel
)
]
)
self.relax_mat_f = scheme.invM * relax_mat_m * scheme.M
# alltogether(self.relax_mat_f)
velocities = sp.Matrix(scheme.stencil.get_all_velocities())
self.velocities = np.asarray(velocities).astype('float')
def tracefreeSym2(rank2_):
"Calculates deviatoric (tracefree symmateric) parts of rank-2 tensor"
rank2_ = rank2_.tomatrix()
return (Rational(1, 2) * (rank2_ + rank2_.transpose())
- Rational(1, 3) * rank2_.trace() * eye(dim))
print(factor(x**2 * z + 4 * x * y * z + 4 * y**2 * z))
print(
sym.integrate(exp(-x**2 - y**2), (x, sym.oo, sym.oo),
(y, -sym.oo, sym.oo)))
print(sym.Solvest(x - 2, x))
print(sym.solvest(Eq(x - 2, 1), x))
m1 = sym.Matrix([[1, 2, 3], [3, 2, 1]])
m2 = sym.Matrix([0, 1, 1])
m2 = sym.Matrix([2, 3, 0])
print(m1)
print(m1 * m2)
print(m2 + m3)
m4 = sym.zeros(2, 3)
m5 = sym.eye(3)
print(m4)
print(m5)
print(sym.ones(3, 2))
print(sym.diag(1, 2, 3))
# ******* Plotting *******
from sympy import symbols
from sympy.plotting import plot
x = symbols('x')
plot(x**2, (x, -5, 5))
plot((x**2, (x, -6, 6)), (x, (x, -5, 5)))
##########################################
def __new__(cls, name, location=None, rotation_matrix=None,
parent=None, vector_names=None, variable_names=None):
"""
The orientation/location parameters are necessary if this system
is being defined at a certain orientation or location wrt another.
Parameters
==========
name : str
The name of the new CoordSysCartesian instance.
location : Vector
The position vector of the new system's origin wrt the parent
instance.
rotation_matrix : SymPy ImmutableMatrix
The rotation matrix of the new coordinate system with respect
to the parent. In other words, the output of
new_system.rotation_matrix(parent).
parent : CoordSys3D
The coordinate system wrt which the orientation/location
(or both) is being defined.
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.
"""
name = str(name)
Vector = sympy.vector.Vector
BaseVector = sympy.vector.BaseVector
Point = sympy.vector.Point
if not isinstance(name, string_types):
raise TypeError("name should be a string")
# If orientation information has been provided, store
# the rotation matrix accordingly
if rotation_matrix is None:
parent_orient = Matrix(eye(3))
else:
if not isinstance(rotation_matrix, Matrix):
raise TypeError("rotation_matrix should be an Immutable" +
"Matrix instance")
parent_orient = rotation_matrix
# If location information is not given, adjust the default
# location as Vector.zero
if parent is not None:
if not isinstance(parent, CoordSys3D):
raise TypeError("parent should be a " +
"CoordSysCartesian/None")
if location is None:
location = Vector.zero
else:
if not isinstance(location, Vector):
raise TypeError("location should be a Vector")
# Check that location does not contain base
# scalars
for x in location.free_symbols:
if isinstance(x, BaseScalar):
raise ValueError("location should not contain" +
" BaseScalars")
origin = parent.origin.locate_new(name + '.origin',
location)
else:
location = Vector.zero
origin = Point(name + '.origin')
# All systems that are defined as 'roots' are unequal, unless
# they have the same name.
# Systems defined at same orientation/position wrt the same
# 'parent' are equal, irrespective of the name.
# This is true even if the same orientation is provided via
# different methods like Axis/Body/Space/Quaternion.
# However, coincident systems may be seen as unequal if
# positioned/oriented wrt different parents, even though
# they may actually be 'coincident' wrt the root system.
if parent is not None:
obj = super(CoordSys3D, cls).__new__(
cls, Symbol(name), location, parent_orient, parent)
else:
obj = super(CoordSys3D, cls).__new__(
cls, Symbol(name), location, parent_orient)
obj._name = name
# Initialize the base vectors
if vector_names is None:
vector_names = (name + '.i', name + '.j', name + '.k')
latex_vects = [(r'\mathbf{\hat{i}_{%s}}' % name),
(r'\mathbf{\hat{j}_{%s}}' % name),
(r'\mathbf{\hat{k}_{%s}}' % name)]
pretty_vects = (name + '_i', name + '_j', name + '_k')
else:
_check_strings('vector_names', vector_names)
vector_names = list(vector_names)
latex_vects = [(r'\mathbf{\hat{%s}_{%s}}' % (x, name)) for
x in vector_names]
pretty_vects = [(name + '_' + x) for x in vector_names]
obj._i = BaseVector(vector_names[0], 0, obj,
pretty_vects[0], latex_vects[0])
obj._j = BaseVector(vector_names[1], 1, obj,
pretty_vects[1], latex_vects[1])
obj._k = BaseVector(vector_names[2], 2, obj,
pretty_vects[2], latex_vects[2])
# Initialize the base scalars
if variable_names is None:
variable_names = (name + '.x', name + '.y', name + '.z')
latex_scalars = [(r"\mathbf{{x}_{%s}}" % name),
(r"\mathbf{{y}_{%s}}" % name),
(r"\mathbf{{z}_{%s}}" % name)]
pretty_scalars = (name + '_x', name + '_y', name + '_z')
else:
_check_strings('variable_names', vector_names)
variable_names = list(variable_names)
latex_scalars = [(r"\mathbf{{%s}_{%s}}" % (x, name)) for
x in variable_names]
pretty_scalars = [(name + '_' + x) for x in variable_names]
obj._x = BaseScalar(variable_names[0], 0, obj,
pretty_scalars[0], latex_scalars[0])
obj._y = BaseScalar(variable_names[1], 1, obj,
pretty_scalars[1], latex_scalars[1])
obj._z = BaseScalar(variable_names[2], 2, obj,
pretty_scalars[2], latex_scalars[2])
obj._h1 = S.One
obj._h2 = S.One
obj._h3 = S.One
obj._transformation_eqs = obj._x, obj._y, obj._y
obj._inv_transformation_eqs = lambda x, y, z: (x, y, z)
# Assign params
obj._parent = parent
if obj._parent is not None:
obj._root = obj._parent._root
else:
obj._root = obj
obj._parent_rotation_matrix = parent_orient
obj._origin = origin
# Return the instance
return obj
def __init__(self, n_qubits, ignore_global):
self.n_qubits = n_qubits
self.matrix_of_circuit = eye(2**n_qubits)
self.ignore_global = ignore_global
oldvars = [
'l_f', 'l_r', 'diff(delta_f(t),t)', 'delta_f(t)', 'diff(v(t),t)', 'v(t)',
'diff(psi(t),t)', 'psi(t)', 'diff(a(t),t)', 'a(t)'
] #10 vars to be replaced
newvars = ['l_f', 'l_r', 'dd', 'd', 'dv', 'v', 'dp', 'p', 'da',
'a'] #10 vars to be replaced
for i in range(0, len(oldvars)):
temp = sym.sympify(oldvars[i])
F = F.subs({temp: newvars[i]})
G = G.subs({temp: newvars[i]})
#### NEED TO ADD I AND CONSIDER LOOP RATE. DISCRETE TIME REQUIREMENTS
des_rate = 50.0
rate = rospy.Rate(des_rate)
F = F / des_rate + sym.eye(F.shape[0])
G = G / des_rate
### All "deriviatves" must be bare-bone variables
Fobj = sym.lambdify((newvars), F, "numpy")
Gobj = sym.lambdify((newvars), G, "numpy")
count = 0 # ADAPTIVE KALMAN FILTER COUNTER
eps = 0 # ADAPTIVE KALMAN FILTER EPS
kfwin = 100 # SAVITZKY GOLAY, KF WIND
svgwin = 25 # SVG WIN
svgorder = 3
kfvect = [] # kf vector of size win
svresults = np.array([kfwin * [0]])
timeInit = rospy.get_time()
print "READY TO BEGIN, START ROSBAG"
while not rospy.is_shutdown():
0, 0, N5, 0, 0, N6, 0, 0, N7, 0, 0, N8], zlist)
B3 = my_map(diff, [N1, N1, N1, N2, N2, N2, N3, N3, N3, N4, N4, N4,\
N5, N5, N5, N6, N6, N6, N7, N7, N7, N8, N8, N8], yxlist)
B4 = my_map(diff, [N1, N1, N1, N2, N2, N2, N3, N3, N3, N4, N4, N4,\
N5, N5, N5, N6, N6, N6, N7, N7, N7, N8, N8, N8], zylist)
B5 = my_map(diff, [N1, N1, N1, N2, N2, N2, N3, N3, N3, N4, N4, N4,\
N5, N5, N5, N6, N6, N6, N7, N7, N7, N8, N8, N8], zxlist)
B = Matrix([B0, B1, B2, B3, B4, B5])
# Create constitutive (material property) matrix:
C = Matrix([[(1 - nu) * g, nu * g, nu * g, 0, 0, 0],
[nu * g, (1 - nu) * g, nu * g, 0, 0, 0],
[nu * g, nu * g, (1 - nu) * g, 0, 0, 0], [0, 0, 0, G, 0, 0],
[0, 0, 0, 0, G, 0], [0, 0, 0, 0, 0, G]])
PI = eye(6)
PH3 = Matrix([\
[y/b, z/c, (y*z)/(b*c), 0, 0, 0, 0, 0, 0, 0, 0, 0],\
[0, 0, 0, x/a, z/c, (x*z)/(a*c), 0, 0, 0, 0, 0, 0],\
[0, 0, 0, 0, 0, 0, x/a, y/b, (x*y)/(a*b), 0, 0, 0],\
[0, 0, 0, 0, 0, 0, 0, 0, 0, z/c, 0, 0],\
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, x/a, 0],\
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, y/b]])
P = PI.row_join(PH3)
tP = P.transpose()
dJ = tP * B
dH = tP * C.inv() * P
# Integration:
print('SymPy is integrating: K for H18B...')
J = dJ.integrate((x, -a, a), (y, -b, b), (z, -c, c))
def _create_moments_matrices(self):
"""
Create the moments matrices M and M^{-1} used to transform the repartition functions into the moments
Three versions of these matrices are computed:
- a sympy version M and invM for each scheme
- a numerical version Mnum and invMnum for each scheme
- a global numerical version MnumGlob and invMnumGlob for all the schemes
"""
M_, invM_, Mu_, Tu_ = [], [], [], []
u_tild = sp.Matrix([rel_ux, rel_uy, rel_uz])
if self.symb_la is not None:
LA = self.symb_la
else:
LA = self.la
compt = 0
for iv, v in enumerate(self.stencil.v):
p = self.P[self.stencil.nv_ptr[iv]:self.stencil.nv_ptr[iv + 1]]
compt += 1
lv = len(v)
M_.append(sp.zeros(lv, lv))
Mu_.append(sp.zeros(lv, lv))
for i in range(lv):
for j in range(lv):
sublist = [(str(self.symb_coord[d]),
sp.Integer(v[j].v[d]) * LA)
for d in range(self.dim)]
M_[-1][i, j] = p[i].subs(sublist)
if self.rel_vel is not None:
sublist = [(str(self.symb_coord[d]),
v[j].v[d] * LA - u_tild[d])
for d in range(self.dim)]
Mu_[-1][i, j] = p[i].subs(sublist)
invM_.append(M_[-1].inv())
Tu_.append(Mu_[-1] * invM_[-1])
gshape = (self.stencil.nv_ptr[-1], self.stencil.nv_ptr[-1])
Tu = sp.eye(gshape[0])
M = sp.zeros(*gshape)
invM = sp.zeros(*gshape)
try:
for k in range(self.nschemes):
nvk = self.stencil.nv[k]
for i in range(nvk):
for j in range(nvk):
index = self.stencil.nv_ptr[
k] + i, self.stencil.nv_ptr[k] + j
M[index] = M_[k][i, j]
invM[index] = invM_[k][i, j]
if self.rel_vel is not None:
Tu[index] = Tu_[k][i, j]
except TypeError:
log.error(
"Unable to convert to float the expression %s or %s.\nCheck the 'parameters' entry.",
M[k][i, j], invM[k][i, j]) #pylint: disable=undefined-loop-variable
sys.exit()
alltogether(Tu, nsimplify=True)
alltogether(M, nsimplify=True)
alltogether(invM, nsimplify=True)
Tmu = Tu.subs(list(zip(u_tild, -u_tild)))
return M, invM, Tu, Tmu
def calc_lbi_nf_state_eq(self, simplify=False):
"""
calc vectorfields fz, and gz of the Lagrange-Byrnes-Isidori-Normalform
instead of the state xx
"""
n = len(self.tt)
nq = len(self.tau)
np = n - nq
nx = 2 * n
# make sure that the system has the desired structure
B = self.eqns.jacobian(self.tau)
cond1 = B[:np, :] == sp.zeros(np, nq)
cond2 = B[np:, :] == -sp.eye(nq)
if not cond1 and cond2:
msg = "The jacobian of the equations of motion do not have the expected structure: %s"
raise NotImplementedError(msg % str(B))
pp = self.tt[:np, :]
qq = self.tt[np:, :]
uu = self.ttd[:np, :]
vv = self.ttd[np:, :]
ww = st.symb_vector('w1:{0}'.format(np + 1))
assert len(vv) == nq
# state w.r.t normal form
self.zz = st.row_stack(qq, vv, pp, ww)
self.ww = ww
# set the actuated accelearations as new inputs
self.aa = self.ttdd[-nq:, :]
# input vectorfield
self.gz = sp.zeros(nx, nq)
self.gz[nq:2 * nq, :] = sp.eye(nq) # identity matrix for the active coordinates
# drift vectorfield (will be completed below)
self.fz = sp.zeros(nx, 1)
self.fz[:nq, :] = vv
self.calc_mass_matrix()
if simplify:
self.M.simplify()
M11 = self.M[:np, :np]
M12 = self.M[:np, np:]
d = M11.berkowitz_det()
adj = M11.adjugate()
if simplify:
d = d.simplify()
adj.simplify()
M11inv = adj / d
# defining equation for ww: ww := uu + M11inv*M12*vv
uu_expr = ww - M11inv * M12 * vv
# setting input tau and acceleration to 0 in the equations of motion
C1K1 = self.eqns[:np, :].subs(st.zip0(self.ttdd, self.tau))
N = st.time_deriv(M11inv * M12, self.tt)
ww_dot = -M11inv * C1K1.subs(lzip(uu, uu_expr)) + N.subs(lzip(uu, uu_expr)) * vv
self.fz[2 * nq:2 * nq + np, :] = uu_expr
self.fz[2 * nq + np:, :] = ww_dot
# how the new coordinates are defined:
self.ww_def = uu + M11inv * M12 * vv
if simplify:
self.fz.simplify()
self.gz.simplify()
self.ww_def.simplify()
def _find_realization(self, G, s):
""" Represenatation [A, B, C, D] of the state space model
Returns the representation in state space of a given transfer function
Parameters
==========
G: Matrix
Matrix valued transfer function G(s) in laplace space
s: symbol
variable s, where G is dependent from
See Also
========
Utils : some quick tools for matrix polynomials
References
==========
Joao P. Hespanha, Linear Systems Theory. 2009.
"""
A, B, C, D = 4 * [None]
try:
m, k = G.shape
except AttributeError:
raise TypeError("G must be a matrix")
# test if G is proper
if not utl.is_proper(G, s, strict=False):
raise ValueError("G must be proper!")
# define D as the limit of G for s to infinity
D = G.limit(s, oo)
# define G_sp as the (stricly proper) difference of G and D
G_sp = simplify(G - D)
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
# get the coefficients of the monic least common denominator of all entries of G_sp
# compute a least common denominator using utl and lcm
lcd = lcm(utl.fraction_list(G_sp, only_denoms=True))
# make it monic
lcd = simplify(lcd / LC(lcd, s))
# and get a coefficient list of its monic. The [1:] cuts the LC away (thats a one)
lcd_coeff = Poly(lcd, s).all_coeffs()[1:]
# get the degree of the lcd
lcd_deg = degree(lcd, s)
# get the Matrix Valued Coeffs of G_sp in G_sp = 1/lcd * (N_1 * s**(n-1) + N_2 * s**(n-2) .. +N_n)
G_sp_coeff = utl.matrix_coeff(simplify(G_sp * lcd), s)
G_sp_coeff = [zeros(m, k)] * (lcd_deg - len(G_sp_coeff)) + G_sp_coeff
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
# now store A, B, C, D in terms of the coefficients of lcd and G_sp
# define A
A = (-1) * lcd_coeff[0] * eye(k)
for alpha in lcd_coeff[1:]:
A = A.row_join((-1) * alpha * eye(k))
for i in xrange(lcd_deg - 1):
if i == 0:
tmp = eye(k)
else:
tmp = zeros(k)
for j in range(lcd_deg)[1:]:
if j == i:
tmp = tmp.row_join(eye(k))
else:
tmp = tmp.row_join(zeros(k))
if tmp is not None:
A = A.col_join(tmp)
# define B
B = eye(k)
for i in xrange(lcd_deg - 1):
B = B.col_join(zeros(k))
# define C
C = G_sp_coeff[0]
for i in range(lcd_deg)[1:]:
C = C.row_join(G_sp_coeff[i])
# return the state space representation
return [simplify(A), simplify(B), simplify(C), simplify(D)]
def __init__(self, N, s=0.5, include_dipole=True, omega_d=2.87, Brms=0):
self.__N = N
self.__s = s
self.__include_dipole = include_dipole
self.omega_d = omega_d # This doesn't do anything
self.__Brms = Brms
if s * 2 % 1 != 0:
raise ValueError('s must be either an integer or a half-integer.')
if s < 0:
raise ValueError('s cannot be negative.')
if (s != 0.5) and (s != 1.0):
raise ValueError('Currently, only s=0.5 and s=1 are supported.')
# Define constants
ge = 2.8 # Gyromagnetic ratio of electron, MHz Gauss^-1
Delta = 2870 # Splitting, MHz (vary this later)
#Bz = 1 # DC z magnetic field, Gauss (vary this later)
# Keep units consistent in the future
#Delta_arr = Delta * np.ones(N) # Array of splittings (in case we want to vary)
Bz_arr = np.random.normal(
0, Brms, size=N) #Bz*np.ones(N)#Bz * np.ones(N) # Array of Bz
# Define array of position tuples (in future versions, make this specify-able or something)
pos_arr = list(
zip(np.random.uniform(-10, 10, N), np.random.uniform(-10, 10, N),
np.random.uniform(-10, 10, N)))
# Generate lists of the spin operators for specific qubits
self.__Sx_arr = []
self.__Sy_arr = []
self.__Sz_arr = []
self.__Sp_arr = []
self.__Sm_arr = []
for ii in range(N): # ii is the qubit the spin operator is for
for jj in range(
N): # jj is the qubit you're evaluating the operator at
if jj == 0:
if ii == jj:
Sx_oper = spin_operator(s, 'x')
Sy_oper = spin_operator(s, 'y')
Sz_oper = spin_operator(s, 'z')
Sp_oper = spin_operator(s, 'p')
Sm_oper = spin_operator(s, 'm')
else:
Sx_oper = sym.eye(int(2 * s + 1))
Sy_oper = sym.eye(int(2 * s + 1))
Sz_oper = sym.eye(int(2 * s + 1))
Sp_oper = sym.eye(int(2 * s + 1))
Sm_oper = sym.eye(int(2 * s + 1))
else:
if ii == jj:
Sx_oper = TensorProduct(Sx_oper, spin_operator(s, 'x'))
Sy_oper = TensorProduct(Sy_oper, spin_operator(s, 'y'))
Sz_oper = TensorProduct(Sz_oper, spin_operator(s, 'z'))
Sp_oper = TensorProduct(Sp_oper, spin_operator(s, 'p'))
Sm_oper = TensorProduct(Sm_oper, spin_operator(s, 'm'))
else:
Sx_oper = TensorProduct(Sx_oper,
sym.eye(int(2 * s + 1)))
Sy_oper = TensorProduct(Sy_oper,
sym.eye(int(2 * s + 1)))
Sz_oper = TensorProduct(Sz_oper,
sym.eye(int(2 * s + 1)))
Sp_oper = TensorProduct(Sp_oper,
sym.eye(int(2 * s + 1)))
Sm_oper = TensorProduct(Sm_oper,
sym.eye(int(2 * s + 1)))
self.__Sx_arr.append(Sx_oper)
self.__Sy_arr.append(Sy_oper)
self.__Sz_arr.append(Sz_oper)
self.__Sp_arr.append(Sp_oper)
self.__Sm_arr.append(Sm_oper)
Sz_scaled = self.__Sz_arr
# Include 2 * N spin-1 splitting terms
H = 0 * Sz_scaled[0]
for ii in range(N):
if s == 0.5:
H += ge * Bz_arr[ii] * Sz_scaled[ii]
#if s == 1:
# H += Delta_arr[ii] * Sz_scaled[ii] ** 2 + ge * Bz_arr[ii] * Sz_scaled[ii]
# Add up matrices to get H
#for ii in range(len(Sz_scaled)):
# H += Sz_scaled[ii]
# If desired, include dipole-dipole interaction terms
# NOTE: Convert dipole moments to RWA at some later time
# Hard code in a single moment for everything, but NOTE let this change later on.
m = 2 * np.pi * 52
if include_dipole:
for ii in range(N):
for jj in range(ii):
pos_i = np.array(pos_arr[ii])
pos_j = np.array(pos_arr[jj])
r = np.sqrt(np.sum((pos_i - pos_j)**2))
rvec = pos_i - pos_j
x = rvec[0]
y = rvec[1]
z = rvec[2]
H += (1 / 2.) * (
m / r**3) * self.__Sp_arr[ii] * self.__Sm_arr[jj]
H += (1 / 2.) * (
m / r**3) * self.__Sm_arr[ii] * self.__Sp_arr[jj]
H += (m / r**3) * self.__Sz_arr[ii] * self.__Sz_arr[jj]
H -= (3 * m / (4 * r**5)) * (
(x**2 + y**2) * self.__Sp_arr[ii] * self.__Sm_arr[jj])
H -= (3 * m / (4 * r**5)) * (
(x**2 + y**2) * self.__Sm_arr[ii] * self.__Sp_arr[jj])
H -= (3 * m / r**5) * (z**2 * self.__Sz_arr[ii] *
self.__Sz_arr[jj])
self.__operator = H
def unitary(self, Ex, Ey, duration):
# Return the sympy unitary matrix for evolution under this Hamiltonian.
# Doesn't return the object in qutip-friendly format
if len(Ex) != len(Ey):
raise ValueError('Ex and Ey do not have the same length.')
steps = np.linspace(0, duration, len(Ex) + 1)
for ii in range(self.N):
if ii == 0:
U = sym.eye(int(2 * self.s + 1))
else:
U = TensorProduct(U, sym.eye(int(2 * self.s + 1)))
# Now add the driving terms
for ii in range(len(Ex)):
t1 = steps[ii]
t2 = steps[ii + 1]
# Build the exponential that goes into the time-evolution operator
# Only the controls hold time-dependence
#int_H_dt = self.__operator * (t2 - t1) # NOTE Need to fix, since dipole operators don't commute with laser
#op = self.__operator
#sx = self.__Sx_arr
#sy = self.__Sy_arr
for jj in range(len(self.__Sx_arr)):
if jj == 0:
Sx_sum = self.__Sx_arr[jj]
Sy_sum = self.__Sy_arr[jj]
else:
Sx_sum += self.__Sx_arr[jj]
Sy_sum += self.__Sy_arr[jj]
int_H_dt = self.__operator * (t2 - t1)
int_H_dt += Ex[ii] * Sx_sum * (t2 - t1)
int_H_dt += Ey[ii] * Sy_sum * (t2 - t1)
#int_H_dt = (Ex[ii] * Sx_sum / self.omega_d) * (np.sin(self.omega_d * t2) - np.sin(self.omega_d * t1))
#int_H_dt += -1 * (Ey[ii] * Sy_sum / self.omega_d) * (np.cos(self.omega_d * t2) - np.cos(self.omega_d * t1))
# Get time-evolution operator over this single slice
# Do this by doing the power series expansion of the matrix exponential (there has to be a better way than this)
# NOTE: In the future, assign a dimension of the matrix and use this instead
P, D = int_H_dt.diagonalize()
exp_D = -1j * D
for ii in range(exp_D.shape[0]):
exp_D[ii, ii] = sym.exp(exp_D[ii, ii])
Uii = P * exp_D * P**-1
U *= Uii
#pdb.set_trace()
#exp_max = 10 # maximum power in the expansion
#for p in range(exp_max + 1):
# print('hi alex')
# pdb.set_trace()
# Uii += np.linalg.matrix_power(-1j * int_H_dt, p) / factorial(p)
#Uii = #sym.exp(-int_H_dt)
#pdb.set_trace()
#if ii == 0:
# U = Uii
#else:
# U = U * Uii
return U
def inverse_T(T):
return T[0:3, 0:3].transpose().row_join(
- T[0:3, 0:3].transpose() * T[0:3, 3]).col_join(
sympy.zeros(1, 3).row_join(sympy.eye(1)))
def find_ini_brian():
import pylab, brian2, brianutils, os, json, sympy, scipy, sys, \
datetime, shelve, autoutils, contextlib
from sympy import S
units= dict(brian2.units.__dict__.items()
+ brian2.units.allunits.__dict__.items()
+ brian2.__dict__.items())
unitlist=["mV","ms","cm2","uF","psiemens","um2","msiemens","cm"]
baseunits=[(k,float(eval(k,units).base)) for k in unitlist]
p = {"simfile" : "sim_DNap_2015-12-14_16:55:14.603625.shv",
"modfile" : "cfg/wangBuzsaki_brian.json", #This is our model
"contfile" : "dat/cont_WB.json", #This is what brian produces and hands to auto
"workdir" : os.getenv("HOME") + "/Uni/CNS/3.Semester/schreiberlab/excitationblock/model0/final",
"dt" : "0.05*ms",
"bifpar" : {
"I" : ["0.0* uA/cm2"],
"Cm" : ["1.0*uF/cm2","1.1*uF/cm2","1.4*uF/cm2","1.41*uF/cm2","1.42*uF/cm2"]
}
}
os.chdir(p["workdir"])
#Now, we define how to load in the model in a nice pythonic way and sort them
#in a way that brian will understand them.
def load_mod(modfile,bp):
nakdic = json.load(open(modfile))
fundic = dict([(j,k.split(":")[0]) for j,k in nakdic["fun"].items()])
pardic = dict([(j,k) for j,k in nakdic["par"].items()])
bifpar = [(k,pardic.pop(k)) for k in bp]
sdelist = [[j,] + k.split(':') for j,k in nakdic['aux_odes'].items()]
sdelist = [(i,":".join((str(S(j).subs(fundic).subs(fundic).subs(pardic)),k))) for i,j,k in sdelist]
sdelist+= [('v', str(sympy.solve(nakdic['current_balance_eq'],'dv/dt')[0].subs(nakdic['currents']).subs(fundic).subs(fundic).subs(pardic))+":volt")]
sde = brian2.Equations("d{}/dt = {}".format(*sdelist[0]))
for i,j in sdelist[1:]:
sde += brian2.Equations("d{}/dt = {}".format(i,j))
return sde
## FIND INITIAL STEADY STATE ##
sde = load_mod(p["modfile"],p['bifpar'].keys())
ode = brianutils.sde2ode(sde)
diffuterms=dict([(k[3:],(S(j).coeff(k)**2).subs(baseunits)) for i,j in sde.eq_expressions for k in sde.stochastic_variables if S(j).coeff(k)!=0])
## ADD PAR ##
for j,k in p["bifpar"].items():
ode += brian2.Equations("{} : {}".format(j,repr(eval(k[0],units).dim)))
brian2.defaultclock.dt = eval(p["dt"], units)
G = brian2.NeuronGroup(1, model=ode, method="rk4",
threshold='not_refractory and (v>5*mV)',
refractory='v>-40*mV')
# PAR INIT #
for j,k in p["bifpar"].items():
setattr(G,j,eval(k[0],units))
# STATE INIT #
G.v= eval("-66 * mV", units)
states = brian2.StateMonitor(G, ode.eq_names, record=True)
spikes = brian2.SpikeMonitor(G)
net = brian2.Network(G,states,spikes)
duration = eval("500 * ms",units)
net.run(duration)
autobifpar = dict([(i,float(eval(j[0],units))) for i,j in p['bifpar'].items()])
## CREATE ADJOINT LINEAR SYSTEM ##
baseunits2 = [('mV', 1), ('ms', 1), ('cm2', 1), ('uF', 1), ('psiemens', 1), ('um2', 1), ('msiemens', 1), ('cm', 1)]
varrhs = [(i,sympy.S(j).subs(baseunits2))
for i,j in ode.eq_expressions]
# varrhs_2 = [(i,sympy.S(j).subs(baseunits))
# for i,j in ode.eq_expressions]
varrhs.sort(cmp=lambda x,y:cmp(x[0],y[0]),reverse=True)
var,rhs = zip(*varrhs);
advar = sympy.S(["ad{}".format(k) for k in var])
J = [[S(i).diff(j) for j in var] for i in rhs]
J = [[j.subs(baseunits) for j in k] for k in J]
adlinsys = [str(k) for k in
(sympy.S("lam")*sympy.eye(len(advar))-sympy.Matrix(J).T)*sympy.Matrix(advar)]
prcnorm=str((sympy.Matrix(sympy.S(advar)).T*sympy.Matrix(sympy.S(rhs)))[0,0] - sympy.S("dotZF/period"))
# Ipar = eval("1.2 * uA/cm2", units) # LC
spikecriterion = [str(S(k).subs([(i,"{}_left".format(i)) for i in var]))
for j,k in zip(var,rhs) if j=="v"]
#Hier kommt das Pythondings von Jan-Hendrik zum Einsatz!
if "A" in autobifpar:
autobifpar.pop("A")
unames,pnames= autoutils.writeFP('tm_new',
bifpar=autobifpar, rhs=rhs, var=var,
bc=['{0}_left-{0}_right'.format(v) for v in var] + spikecriterion,
ic=[])
inivals = ([float(getattr(states,j)[0][-1]) for j in var])
#convert first value (V) from V to mV.
inivals[0] *= 1000
return unames, pnames, inivals, autobifpar
from __future__ import print_function, division
from sympy import zeros, eye, Symbol, solve_linear_system
from sympy.core.compatibility import range
N = 8
M = zeros(N, N + 1)
M[:, :N] = eye(N)
S = [Symbol('A%i' % i) for i in range(N)]
def timeit_linsolve_trivial():
solve_linear_system(M, *S)
def CubicElasticityRelations(ConstantsType):
# Define Symbols
E0, Nu0, Mu0, Lambda0, Lambda0p = sp.symbols(
r'\epsilon_{0} \nu_{0} \mu_{0} \lambda_{0} \lambda_{0}^{`}')
Rho, m1, m2, m3, k, l = sp.symbols(r'\rho m_{1} m_{2} m_{3} k l')
EigenValues = np.array([m1, m2, m3])
# Define Eigenvectors
I = sp.eye(3)
vm1, vm2, vm3 = I[:, 0], I[:, 1], I[:, 2]
EigenVectors = np.array([vm1, vm2, vm3])
# Build Orthotropic Compliance Tensor
ComplianceTensor = sp.zeros(9)
for i in range(3):
Mi = DyadicProduct(EigenVectors[i], EigenVectors[i])
Part1 = 1 / (E0 * Rho**k * EigenValues[i]**(2 * l)) * DyadicProduct(
Mi, Mi)
ComplianceTensor += IsoMorphism3333_99(Part1)
for ii in range(3 - 1):
j = i - ii - 1
Mj = DyadicProduct(EigenVectors[j], EigenVectors[j])
Part2 = -Nu0 / (E0 * Rho**k * EigenValues[i]**l *
EigenValues[j]**l) * DyadicProduct(Mi, Mj)
Part3 = 1 / (2 * Mu0 * Rho**k * EigenValues[i]**l *
EigenValues[j]**l) * SymmetricProduct(Mi, Mj)
ComplianceTensor += IsoMorphism3333_99(Part2 + Part3)
ComplianceTensor = IsoMorphism99_66(ComplianceTensor)
# Compute Inverse Compliance Matrix (= Stiffness Matrix)
ComplianceTensorInv = sp.Matrix(ComplianceTensor).inv()
# Build Orthotropic Stiffness Matrix
StiffnessTensor = sp.zeros(9)
for i in range(3):
Mi = DyadicProduct(EigenVectors[i], EigenVectors[i])
Part1 = (Lambda0 + 2 * Mu0) * Rho**k * EigenValues[i]**(
2 * l) * DyadicProduct(Mi, Mi)
StiffnessTensor += IsoMorphism3333_99(Part1)
for ii in range(3 - 1):
j = i - ii - 1
Mj = DyadicProduct(EigenVectors[j], EigenVectors[j])
Part2 = Lambda0p * Rho**k * EigenValues[i]**l * EigenValues[
j]**l * DyadicProduct(Mi, Mj)
Part3 = 2 * Mu0 * Rho**k * EigenValues[i]**l * EigenValues[
j]**l * SymmetricProduct(Mi, Mj)
StiffnessTensor += IsoMorphism3333_99(Part2 + Part3)
StiffnessTensor = IsoMorphism99_66(StiffnessTensor)
# Express Relation
Equation = StiffnessTensor - ComplianceTensorInv
if ConstantsType == 'Engineering Constants':
Solution = sp.solve(Equation, Lambda0, Lambda0p)
Relation1 = Solution[Lambda0]
Relation2 = Solution[Lambda0p]
elif ConstantsType == 'Lamé Constants':
Solution = sp.solve(Equation, E0, Mu0)
Relation1 = Solution[E0]
Relation2 = Solution[Mu0]
return Relation1, Relation2
def test_kahane_simplify1():
i0,i1,i2,i3,i4,i5,i6,i7,i8,i9,i10,i11,i12,i13,i14,i15 = tensor_indices('i0:16', LorentzIndex)
mu, nu, rho, sigma = tensor_indices("mu, nu, rho, sigma", LorentzIndex)
D = 4
t = G(i0)*G(i1)
r = kahane_simplify(t)
assert r.equals(t)
t = G(i0)*G(i1)*G(-i0)
r = kahane_simplify(t)
assert r.equals(-2*G(i1))
t = G(i0)*G(i1)*G(-i0)
r = kahane_simplify(t)
assert r.equals(-2*G(i1))
t = G(i0)*G(i1)
r = kahane_simplify(t)
assert r.equals(t)
t = G(i0)*G(i1)
r = kahane_simplify(t)
assert r.equals(t)
t = G(i0)*G(-i0)
r = kahane_simplify(t)
assert r.equals(4*eye(4))
t = G(i0)*G(-i0)
r = kahane_simplify(t)
assert r.equals(4*eye(4))
t = G(i0)*G(-i0)
r = kahane_simplify(t)
assert r.equals(4*eye(4))
t = G(i0)*G(i1)*G(-i0)
r = kahane_simplify(t)
assert r.equals(-2*G(i1))
t = G(i0)*G(i1)*G(-i0)*G(-i1)
r = kahane_simplify(t)
assert r.equals((2*D - D**2)*eye(4))
t = G(i0)*G(i1)*G(-i0)*G(-i1)
r = kahane_simplify(t)
assert r.equals((2*D - D**2)*eye(4))
t = G(i0)*G(-i0)*G(i1)*G(-i1)
r = kahane_simplify(t)
assert r.equals(16*eye(4))
t = (G(mu)*G(nu)*G(-nu)*G(-mu))
r = kahane_simplify(t)
assert r.equals(D**2*eye(4))
t = (G(mu)*G(nu)*G(-nu)*G(-mu))
r = kahane_simplify(t)
assert r.equals(D**2*eye(4))
t = (G(mu)*G(nu)*G(-nu)*G(-mu))
r = kahane_simplify(t)
assert r.equals(D**2*eye(4))
t = (G(mu)*G(nu)*G(-rho)*G(-nu)*G(-mu)*G(rho))
r = kahane_simplify(t)
assert r.equals((4*D - 4*D**2 + D**3)*eye(4))
t = (G(-mu)*G(-nu)*G(-rho)*G(-sigma)*G(nu)*G(mu)*G(sigma)*G(rho))
r = kahane_simplify(t)
assert r.equals((-16*D + 24*D**2 - 8*D**3 + D**4)*eye(4))
t = (G(-mu)*G(nu)*G(-rho)*G(sigma)*G(rho)*G(-nu)*G(mu)*G(-sigma))
r = kahane_simplify(t)
assert r.equals((8*D - 12*D**2 + 6*D**3 - D**4)*eye(4))
# Expressions with free indices:
t = (G(mu)*G(nu)*G(rho)*G(sigma)*G(-mu))
r = kahane_simplify(t)
assert r.equals(-2*G(sigma)*G(rho)*G(nu))
t = (G(mu)*G(nu)*G(rho)*G(sigma)*G(-mu))
r = kahane_simplify(t)
assert r.equals(-2*G(sigma)*G(rho)*G(nu))
def D_i(vec):
[Ji, Mi, Ii, Ri] = vec
Jv = Ji[:3, :]
Jw = Ji[3:, :]
Ri = sy.eye(3)
return Jv.T * Mi * Jv + Jw.T * Ri * Ii * Ri.T * Jw
def add_delta(ne):
return ne * eye(4) # DiracSpinorIndex.delta(DiracSpinorIndex.auto_left, -DiracSpinorIndex.auto_right)
(sym.conjugate(b) - b): 2 * I * bi
})
#Lreal = Lreal.subs(a,ar+I*ai)
Lrealfunc = sym.lambdify(
(ar, ai, br, bi, delo, delm, delao, delam, gamma13, gamma23, gamma2d,
gamma3d, nbath, gammamu, Omega, go, gm), Lreal)
H_disc_diff = sym.diff(
sym.discriminant(
sym.det(
H_sys.subs({
a: 0,
delao - delo: delta3,
delam - delm: delta2
}) - lam * sym.eye(3)), lam), delta3)
H_disc_diff_symfun = sym.lambdify((delta2, delta3, b, gm, Omega), H_disc_diff)
H_disc_diff_fun = lambda delta2, delta3, b, gm, Omega: np.array(
H_disc_diff_symfun(delta2, delta3, b, gm, Omega).real, dtype=float)
def find_dressed_states_m(delaoval, deloval, delmval, bval, p):
try:
deltam_ds_val = scipy.optimize.fsolve(
(H_disc_diff_fun),
np.array(delmval),
args=(delaoval - deloval, bval, p['gm'], p['Omega'])) + delmval
except ValueError:
deltam_ds_val = np.nan
except TypeError:
deltam_ds_val = np.nan
# [0,0,0,0,0,1,0,1,0],
# [0,0,0,0,0,I,0,-I,0]
# ])
# CtoR=CtoR/2
# Lreal = sym.simplify(CtoR*L*CtoR.inv())
# #ar,ai,br,bi,gmr,gmi,gor,goi=sym.symbols('a_r a_i b_r b_i g_mu_r g_mu_i g_o_r g_o_i')
# #agor,agoi, bgmr, bgmi,Wr,Wi=sym.symbols('ag_or ag_oi bg_mu_r bg_mu_i Omega_r Omega_i')
# ar,ai,br,bi=sym.symbols('a_r a_i b_r b_i ')
# #Lreal.subs({agor:(a*go+sym.conjugate(a)*sym.conjugate(go))/2,I*(sym.conjugate(a)*sym.conjugate(go)-a*go)/2:agoi})
# #Lreal=Lreal.subs({(a+sym.conjugate(a)):2*ar,(sym.conjugate(a)-a):2*I*ai,(b+sym.conjugate(b)):2*br,(sym.conjugate(b)-b):2*I*bi})
# #Lreal = Lreal.subs(a,ar+I*ai)
#
# #Lrealfunc = sym.lambdify((ar,ai,br,bi,delo, delm,delao, delam, gamma13, gamma23, gamma2d, gamma3d, nbath,gammamu,Omega,go,gm),Lreal)
H_disc_diff=sym.diff(sym.discriminant(sym.det(H_sys.subs({a:0,delao-delo:delta3,delam-delm:delta2})-lam*sym.eye(3)),lam),delta3)
H_disc_diff_symfun=sym.lambdify((delta2,delta3,b,gm,Omega),H_disc_diff)
H_disc_diff_fun=lambda delta2,delta3,b,gm,Omega: np.array(H_disc_diff_symfun(delta2,delta3,b,gm,Omega).real,dtype=float)
def find_dressed_states_m(delaoval, deloval,delmval,bval,p):
try:
deltam_ds_val=scipy.optimize.fsolve((H_disc_diff_fun),np.array(delmval),args=(delaoval-deloval,bval,p['gm'],p['Omega']))+delmval
except ValueError:
deltam_ds_val=np.nan
except TypeError:
deltam_ds_val=np.nan
#except NameError:
# deltam_ds_val=np.nan
return deltam_ds_val
def orient_new(self, name, orienters, location=None,
vector_names=None, variable_names=None):
"""
Creates a new CoordSysCartesian 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 CoordSysCartesian 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 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)
def spre(m):
return TensorProduct(sym.eye(m.shape[0]),m)
def test_smith_normal_form_3():
m = sympy.SparseMatrix(5, 3, {(0, 1): 1, (1, 0): 1, (4, 2): 1})
mp, _, _ = smith_normal_form(m)
assert mp.shape == (3, 3)
assert (mp - sympy.eye(3)).is_zero
def spost(m):
return TensorProduct(m.T, sym.eye(m.shape[0]))
def test_orient_explicit():
A = ReferenceFrame('A')
B = ReferenceFrame('B')
A.orient_explicit(B, eye(3))
assert A.dcm(B) == Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
from sympy import ImmutableMatrix, Matrix, eye, zeros
from sympy.abc import x, y
from sympy.utilities.pytest import raises
IM = ImmutableMatrix([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
ieye = ImmutableMatrix(eye(3))
def test_immutable_creation():
assert IM.shape == (3, 3)
assert IM[1, 2] == 6
assert IM[2, 2] == 9
def test_immutability():
with raises(TypeError):
IM[2, 2] = 5
def test_slicing():
assert IM[1, :] == ImmutableMatrix([[4, 5, 6]])
assert IM[:2, :2] == ImmutableMatrix([[1, 2], [4, 5]])
def test_subs():
A = ImmutableMatrix([[1, 2], [3, 4]])
B = ImmutableMatrix([[1, 2], [x, 4]])
C = ImmutableMatrix([[-x, x * y], [-(x + y), y**2]])
assert B.subs(x, 3) == A
assert (x * B).subs(x, 3) == 3 * A
assert (x * eye(2) + B).subs(x, 3) == 3 * eye(2) + A
import sympy as sm
sm.init_printing()
exchange_table_t = sm.Matrix([
[0.2,0.3,0.5],
[0.8,0.1,0.1],
[0.4,0.4,0.2]
])
exchange_table = exchange_table_t.transpose()
sm.pprint(exchange_table)
m=sm.eye(3) - exchange_table
sm.pprint(m)
b=sm.zeros(3).col(0)
sm.pprint(b)
m=m.col_insert(3,b)
sm.pprint(m)
sm.pprint(m.rref())
from sympy.solvers.solveset import linsolve
x,y,z=sm.symbols("x y z")
result=linsolve(m,x,y,z)
def reflection_matrix(v):
return (eye(len(v)) - 2 * v.T * v / v.dot(v)).as_immutable()
def __init__(self, robotdef, ifunc=None):
if not ifunc:
ifunc = _id
self.rbtdef = robotdef
self.dof = self.rbtdef.dof
def inverse_T(T):
return T[0:3, 0:3].transpose().row_join(
- T[0:3, 0:3].transpose() * T[0:3, 3]).col_join(
sympy.zeros(1, 3).row_join(sympy.eye(1)))
(alpha, a, d, theta) = sympy.symbols('alpha,a,d,theta', real=True)
self.Tdh = list(range(self.rbtdef.dof))
self.Tdh_inv = list(range(self.rbtdef.dof))
self.Rdh = list(range(self.rbtdef.dof))
self.pdh = list(range(self.rbtdef.dof))
dh_transfmat_inv = inverse_T(self.rbtdef._dh_transfmat)
q_subs = dict([(_joint_i_symb(i + 1), self.rbtdef.q[i])
for i in range(self.rbtdef.dof)])
for l in range(self.rbtdef.dof):
subs_dict = dict(
zip(self.rbtdef._dh_symbols, self.rbtdef._dh_parms[l]))
self.Tdh[l] = self.rbtdef._dh_transfmat.subs(
subs_dict).subs(q_subs)
self.Tdh_inv[l] = dh_transfmat_inv.subs(subs_dict).subs(q_subs)
self.Rdh[l] = self.Tdh[l][0:3, 0:3]
self.pdh[l] = self.Tdh[l][0:3, 3]
self.T = list(range(self.rbtdef.dof))
# set T[-1] so that T[l-1] for l=0 is correctly assigned
# T[-1] is override after
self.T[-1] = sympy.eye(4)
for l in range(self.rbtdef.dof):
self.T[l] = ifunc(self.T[l - 1] * self.Tdh[l])
self.R = list(range(self.rbtdef.dof))
self.p = list(range(self.rbtdef.dof))
self.z = list(range(self.rbtdef.dof))
for l in range(self.rbtdef.dof):
self.R[l] = self.T[l][0:3, 0:3]
self.p[l] = self.T[l][0:3, 3]
self.z[l] = self.R[l][0:3, 2]
#
# for screw theory:
if self.rbtdef._dh_convention == 'standard':
cos = sympy.cos
sin = sympy.sin
Mr = sympy.Matrix([[1, 0, 0, a],
[0, cos(alpha), -sin(alpha), 0],
[0, sin(alpha), cos(alpha), d],
[0, 0, 0, 1]])
Pr = sympy.Matrix([[0, -1, 0, 0],
[1, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0]])
# from Frank Park paper:
# Mp = sympy.Matrix(
# [[cos(theta), -sin(theta) * cos(alpha), 0, a * cos(theta)],
# [sin(theta), cos(theta) * cos(alpha), -sin(alpha),
# a * sin(theta)],
# [0, sin(alpha), cos(alpha), 0],
# [0, 0, 0, 1]])
# my own:
Mp = sympy.Matrix(
[[cos(theta), -sin(theta) * cos(alpha),
sin(theta) * sin(alpha), a * cos(theta)],
[sin(theta), cos(theta) * cos(alpha),
-cos(theta) * sin(alpha), a * sin(theta)],
[0, sin(alpha), cos(alpha), 0],
[0, 0, 0, 1]])
Pp = sympy.Matrix([[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 1],
[0, 0, 0, 0]])
# if 0:
# D_exp2trig = { exp(SR.wild(0)) : exp(SR.wild(0).real_part())
# * ( cos(SR.wild(0).imag_part()) +
# sympy.I*sin(SR.wild(0).imag_part()) ) }
# ePtM_r = exp( Pr*theta ) * Mr
# ePtM_r = ePtM_r.expand().subs(D_exp2trig).simplify_rational()
# ePtM_p = exp( Pp*d ) * Mp
# ePtM_p = ePtM_p.expand().subs(D_exp2trig).simplify_rational()
# if bool( ePtM_r != self.rbtdef._dh_transfmat or ePtM_p !
# self.rbtdef._dh_transfmat ):
# raise Exception('Geom: interlink transformation does not
# follows implemented DH formulation')
Sr = (inverse_T(Mr) * Pr * Mr).applyfunc(
lambda x: sympy.trigsimp(x))
Sp = (inverse_T(Mp) * Pp * Mp).applyfunc(
lambda x: sympy.trigsimp(x))
def sym_se3_unskew(g):
w = sympy.Matrix([g[2, 1], g[0, 2], g[1, 0]])
v = g[0:3, 3]
return w.col_join(v)
self.S = list(range(self.rbtdef.dof))
for l in range(self.rbtdef.dof):
if self.rbtdef._links_sigma[l]:
S = Sp
else:
S = Sr
self.S[l] = ifunc(sym_se3_unskew(S.subs(dict(zip(
self.rbtdef._dh_symbols, self.rbtdef._dh_parms[l])))))
def _q_tensor(grid, ei):
return ei.transpose().multiply(ei) - sympy.eye(grid.dim) * grid.cssq