def get_dixon_polynomial(self):
r"""
Returns
=======
dixon_polynomial: polynomial
Dixon's polynomial is calculated as:
delta = Delta(A) / ((x_1 - a_1) ... (x_n - a_n)) where,
A = |p_1(x_1,... x_n), ..., p_n(x_1,... x_n)|
|p_1(a_1,... x_n), ..., p_n(a_1,... x_n)|
|... , ..., ...|
|p_1(a_1,... a_n), ..., p_n(a_1,... a_n)|
"""
if self.m != (self.n + 1):
raise ValueError('Method invalid for given combination.')
# First row
rows = [self.polynomials]
temp = list(self.variables)
for idx in range(self.n):
temp[idx] = self.dummy_variables[idx]
substitution = {var: t for var, t in zip(self.variables, temp)}
rows.append([f.subs(substitution) for f in self.polynomials])
A = Matrix(rows)
terms = zip(self.variables, self.dummy_variables)
product_of_differences = Mul(*[a - b for a, b in terms])
dixon_polynomial = (A.det() / product_of_differences).factor()
return poly_from_expr(dixon_polynomial, self.dummy_variables)[0]
def linear_rref(A, b, Matrix=None, S=None):
""" Transform a linear system to reduced row-echelon form
Transforms both the matrix and right-hand side of a linear
system of equations to reduced row echelon form
Parameters
----------
A: Matrix-like
iterable of rows
b: iterable
Returns
-------
A', b' - transformed versions
"""
if Matrix is None:
from sympy import Matrix
if S is None:
from sympy import S
mat_rows = [_map2l(S, list(row) + [v]) for row, v in zip(A, b)]
aug = Matrix(mat_rows)
raug, pivot = aug.rref()
nindep = len(pivot)
return raug[:nindep, :-1], raug[:nindep, -1]
class MatrixGenerator:
def __init__(self, row_size):
first_row = [0] * row_size
self.matrix = Matrix([first_row])
self.has_added_row = False
self.count = 0
def AddRow(self, row):
self.matrix = self.matrix.row_insert(0, Matrix([row]))
self.count += 1
if not self.has_added_row:
self.matrix.row_del(1)
self.has_added_row = True
def _compute_alpha_wrench(model, robo, j):
"""
Compute the wrench as a function of tau - joint torque without
friction.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: joint number
Returns:
An instance of DynModel that contains all the new values.
"""
j_alpha_j = Screw()
# local variables
j_a_j = robo.geos[j].axisa
j_k_j = model.no_qddot_inertias[j].val
j_gamma_j = model.gammas[j].val
j_inertia_j_s = model.star_inertias[j].val
j_beta_j_s = model.star_betas[j].val
h_j = Matrix([model.joint_inertias[j]])
tau_j = Matrix([model.taus[j]])
# actual computation
j_alpha_j.val = (j_k_j * j_gamma_j) + \
(j_inertia_j_s * j_a_j * h_j.inv() * \
(tau_j + j_a_j.transpose() * j_beta_j_s)) - j_beta_j_s
# store in model
model.alphas[j] = j_alpha_j
return model
def _compute_joint_acceleration(model, robo, j, i):
"""
Compute the joint acceleration for joint j.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
i: antecedent value
Returns:
An instance of DynModel that contains all the new values.
"""
# local variables
j_a_j = robo.geos[j].axisa
j_s_i = robo.geos[j].tmat.s_j_wrt_i
j_gamma_j = model.gammas[j].val
j_inertia_j_s = model.star_inertias[j].val
j_beta_j_s = model.star_betas[j].val
h_j = Matrix([model.joint_inertias[j]])
tau_j = Matrix([model.taus[j]])
i_vdot_i = model.accels[i].val
# actual computation
qddot_j = h_j.inv() * (-j_a_j.transpose() * j_inertia_j_s * \
((j_s_i * i_vdot_i) + j_gamma_j) + tau_j + \
(j_a_j.transpose() * j_beta_j_s))
# store in model
model.qddots[j] = qddot_j[0, 0]
return model
def generalized_active_forces_V(V, q, u, kde_map, vc_map=None):
"""Returns a list of the generalized active forces using equation 5.1.18
from Kane 1985.
'V' is a potential energy function for the system.
'q' is a list of generalized coordinates.
'u' is a list of the independent generalized speeds.
'kde_map' is a dictionary with q dots as keys and the equivalent
expressions in terms of q's and u's as values.
'vc_map' is a dictionay with the dependent u's as keys and the expression
in terms of independent u's as values.
"""
n = len(q)
p = len(u)
m = n - p
if vc_map is None:
A_kr = Matrix.zeros(m, p)
else:
A_kr, _ = vc_matrix(u, vc_map)
u_ = u + sorted(vc_map.keys(), cmp=lambda x, y: x.compare(y))
W_sr, _ = kde_matrix(u_, kde_map)
dV_dq = map(lambda x: diff(V, x), q)
Fr = Matrix.zeros(1, p)
for s in range(n):
Fr -= dV_dq[s] * (W_sr[s, :p] + W_sr[s, p:]*A_kr[:, :p])
return Fr[:]
def test_extract():
m = Matrix(4, 3, lambda i, j: i*3 + j)
assert m.extract([0,1,3],[0,1]) == Matrix(3,2,[0,1,3,4,9,10])
assert m.extract([0,3],[0,0,2]) == Matrix(2,3,[0,0,2,9,9,11])
assert m.extract(range(4),range(3)) == m
raises(IndexError, 'm.extract([4], [0])')
raises(IndexError, 'm.extract([0], [3])')
def test_symbolic_twoport():
circuit.default_toolkit = symbolic
cir = SubCircuit()
k = symbolic.kboltzmann
var('R1 R0 C1 w T', real=True, positive=True)
s = 1j*w
cir['R0'] = R(1, gnd, r=R0)
cir['R1'] = R(1, 2, r=R1)
# cir['C1'] = C(2, gnd, c=C1)
## Add an AC source to verify that the source will not affect results
# cir['IS'] = IS(1, gnd, iac=1)
## Run symbolic 2-port analysis
twoport_ana = TwoPortAnalysis(cir, Node('1'), gnd, Node('2'), gnd,
noise = True, toolkit=symbolic,
noise_outquantity = 'v')
result = twoport_ana.solve(freqs=s, complexfreq=True)
ABCD = Matrix(result['twoport'].A)
ABCD.simplify()
assert_array_equal(ABCD, np.array([[1 + 0*R1*C1*s, R1],
[(1 + 0*R0*C1*s + 0*R1*C1*s) / R0, (R0 + R1)/R0]]))
assert_array_equal(simplify(result['Sin'] - (4*k*T/R0 + 4*R1*k*T/R0**2)), 0)
assert_array_equal(simplify(result['Svn']), 4*k*T*R1)
def generalized_inertia_forces_K(K, q, u, kde_map, vc_map=None):
"""Returns a list of the generalized inertia forces using equation 5.6.6
from Kane 1985.
'K' is a potential energy function for the system.
'q' is a list of generalized coordinates.
'u' is a list of the independent generalized speeds.
'kde_map' is a dictionary with q dots as keys and the equivalent
expressions in terms of q's and u's as values.
'vc_map' is a dictionay with the dependent u's as keys and the expression
in terms of independent u's as values.
"""
n = len(q)
p = len(u)
m = n - p
t = symbols('t')
if vc_map is None:
A_kr = Matrix.zeros(m, p)
else:
A_kr, _ = vc_matrix(u, vc_map)
u_ = u + sorted(vc_map.keys(), cmp=lambda x, y: x.compare(y))
W_sr, _ = kde_matrix(u_, kde_map)
qd = map(lambda x: x.diff(t), q)
K_partial_term = [K.diff(qd_s).diff(t) - K.diff(q_s)
for qd_s, q_s in zip(qd, q)]
K_partial_term = subs(K_partial_term, kde_map)
if vc_map is not None:
K_partial_term = subs(K_partial_term, vc_map)
Fr_star = Matrix.zeros(1, p)
for s in range(n):
Fr_star -= K_partial_term[s] * (W_sr[s, :p] + W_sr[s, p:]*A_kr[:, :p])
return Fr_star[:]
def _form_permutation_matrices(self):
"""Form the permutation matrices Pq and Pu."""
# Extract dimension variables
l, m, n, o, s, k = self._dims
# Compute permutation matrices
if n != 0:
self._Pq = permutation_matrix(self.q, Matrix([self.q_i, self.q_d]))
if l > 0:
self._Pqi = self._Pq[:, :-l]
self._Pqd = self._Pq[:, -l:]
else:
self._Pqi = self._Pq
self._Pqd = Matrix()
if o != 0:
self._Pu = permutation_matrix(self.u, Matrix([self.u_i, self.u_d]))
if m > 0:
self._Pui = self._Pu[:, :-m]
self._Pud = self._Pu[:, -m:]
else:
self._Pui = self._Pu
self._Pud = Matrix()
# Compute combination permutation matrix for computing A and B
P_col1 = Matrix([self._Pqi, zeros(o + k, n - l)])
P_col2 = Matrix([zeros(n, o - m), self._Pui, zeros(k, o - m)])
if P_col1:
if P_col2:
self.perm_mat = P_col1.row_join(P_col2)
else:
self.perm_mat = P_col1
else:
self.perm_mat = P_col2
def tet4():
N1 = z
N2 = n
N3 = b
N4 = 1 - z - n - b
X = Matrix([[1, x, y, z]])
X2 = Matrix([[1, x1, y1, z1],
[1, x2, y2, z2],
[1, x3, y3, z3],
[1, x4, y4, z4]])
N = X * X2.inv()
N1 = N[0, 0]
N2 = N[0, 1]
N3 = N[0, 2]
N4 = N[0, 3]
N = Matrix([[N1, 0, 0, N2, 0, 0, N3, 0, 0, N4, 0, 0],
[0, N1, 0, 0, N2, 0, 0, N3, 0, 0, N4, 0],
[0, 0, N1, 0, 0, N2, 0, 0, N3, 0, 0, N4]])
NT = N.transpose()
#pdV = p*v
pdV = 3
factorI = 1
M = makeM(pdV, NT, factorI, levels=3)
print "Mtet = \n", M
def test_hessenberg():
A = Matrix([[3, 4, 1],[2, 4 ,5],[0, 1, 2]])
assert A.is_upper_hessenberg()
assert A.transpose().is_lower_hessenberg()
A = Matrix([[3, 4, 1],[2, 4 ,5],[3, 1, 2]])
assert not A.is_upper_hessenberg()
def _kindiffeq(self, kdeqs):
"""Supply all the kinematic differential equations in a list.
They should be in the form [Expr1, Expr2, ...] where Expri is equal to
zero
Parameters
==========
kdeqs : list (of Expr)
The listof kinematic differential equations
"""
if len(self._q) != len(kdeqs):
raise ValueError('There must be an equal number of kinematic '
'differential equations and coordinates.')
uaux = self._uaux
# dictionary of auxiliary speeds which are equal to zero
uaz = dict(zip(uaux, [0] * len(uaux)))
kdeqs = Matrix(kdeqs).subs(uaz)
qdot = self._qdot
qdotzero = dict(zip(qdot, [0] * len(qdot)))
u = self._u
uzero = dict(zip(u, [0] * len(u)))
f_k = kdeqs.subs(uzero).subs(qdotzero)
k_kqdot = (kdeqs.subs(uzero) - f_k).jacobian(Matrix(qdot))
k_ku = (kdeqs.subs(qdotzero) - f_k).jacobian(Matrix(u))
self._k_ku = self._mat_inv_mul(k_kqdot, k_ku)
self._f_k = self._mat_inv_mul(k_kqdot, f_k)
self._k_kqdot = eye(len(qdot))
def __init__(self, frame):
"""Supply the inertial frame for Kane initialization. """
# Big storage things
self._inertial = frame
self._forcelist = None
self._bodylist = None
self._fr = None
self._frstar = None
self._rhs = None
self._aux_eq = None
# States
self._q = None
self._qdep = []
self._qdot = None
self._u = None
self._udep = []
self._udot = None
self._uaux = None
# Differential Equations Matrices
self._k_d = None
self._f_d = None
self._k_kqdot = None
self._k_ku = None
self._f_k = None
# Constraint Matrices
self._f_h = Matrix([])
self._k_nh = Matrix([])
self._f_nh = Matrix([])
self._k_dnh = Matrix([])
self._f_dnh = Matrix([])
def Newton_solver_pq(error, hx, ht, gx, gt, x0):
p, q = symbols('p q')
epsilon1 = gx*hx[-2]**p + gt*ht[-2]**q - error[-2]
epsilon2 = gx*hx[-1]**p + gt*ht[-1]**q - error[-1]
epsilon = [epsilon1, epsilon2]
x = [p, q]
def f(i,j):
return diff(epsilon[i], x[j])
Jacobi = Matrix(2, 2, f)
JacobiInv = Jacobi.inv()
epsilonfunc = lambdify(x, epsilon)
JacobiInvfunc = lambdify(x, JacobiInv)
x = x0
F = np.asarray(epsilonfunc(x))
for n in range(8):
Jinv = np.asarray(JacobiInvfunc(x))
F = np.asarray(epsilonfunc(x))
x = x - np.dot(Jinv, F)
return x
def Newton_solver_gxgt(error, hx, ht, x0, p_value=1, q_value=1):
gx, gt, p, q, hx1, hx2, ht1, ht2 = symbols('gx gt p q qhx1 hx2 ht1 ht2')
epsilon1 = gx*hx1**p + gt*ht1**q - error[-2]
epsilon2 = gx*hx2**p + gt*ht2**q - error[-1]
epsilon = [epsilon1, epsilon2]
x = [gx, gt]
knowns = [p, q, hx1, hx2, ht1, ht2]
def f(i,j):
return diff(epsilon[i], x[j])
Jacobi = Matrix(2, 2, f)
JacobiInv = Jacobi.inv()
epsilonfunc = lambdify([x, knowns], epsilon)
JacobiInvfunc = lambdify([x, knowns], JacobiInv)
x = x0
knowns = [p_value, q_value, hx[-2], hx[-1], ht[-2], ht[-1]]
F = np.asarray(epsilonfunc(x, knowns))
for n in range(8):
Jinv = np.asarray(JacobiInvfunc(x, knowns))
F = np.asarray(epsilonfunc(x, knowns))
x = x - np.dot(Jinv, F)
print "n, x: ", n, x
return x
def test_Matrix_sum():
x, y, z = Symbol('x'), Symbol('y'), Symbol('z')
M = Matrix([[1,2,3],[x,y,x],[2*y,-50,z*x]])
m = matrix([[2,3,4],[x,5,6],[x,y,z**2]])
assert M+m == Matrix([[3,5,7],[2*x,y+5,x+6],[2*y+x,y-50,z*x+z**2]])
assert m+M == Matrix([[3,5,7],[2*x,y+5,x+6],[2*y+x,y-50,z*x+z**2]])
assert M+m == M.add(m)
def main():
fp=open('clusters','w')
fp2=open('eigenvalues and best cluster number k','w')
fp3=open('cluster sizes','w')
trainingSet=[]
loadDataset1('roadNet-PA.csv',trainingSet)
G=Graph_build(trainingSet)
print len(G.nodes())
N_to_M, M_to_N=Node_num_and_matrix_num(G)
similarity_matrix=similarity_cal(G,N_to_M)
degree_matrix=degree_cal(similarity_matrix)
L_matrix=L_cal(degree_matrix,similarity_matrix)
M=Matrix(L_matrix)
print 'start eigenvectors'
MM=M.eigenvects()
print 'end eigenvectors'
v,w=get_eigen(L_matrix)
L=len(G.nodes())
k=get_k(MM)
w=get_new_eigenvectors(MM,w)
for x in range(len(v)):
fp2.write(repr(v[x])+'\n')
Y=get_Y(w,L,k)
Y_pred=KMeans(n_clusters=k, max_iter=600).fit_predict(Y)
clusterList=clusters_cal(k,Y_pred,M_to_N)
for x in range(len(clusterList)):
fp.write('This is '+repr(x)+'th cluster and it has members:\n')
fp.write(repr(clusterList[x])+'\n\n\n\n')
fp3.write(repr(len(clusterList[x]))+'\n')
for x in range(len(clusterList)):
G_new=new_Graph_build(G,clusterList[x])
random_mqi(G_new,x)
def spin_vector_to_state(spin):
from sympy import Matrix
from sglib_v0 import sigma_x, sigma_y, sigma_z, find_eigenvectors
from numpy import shape
# The spin vector must have three components.
if shape(spin) == () or len(spin) != 3:
print('Error: spin vector must have three elements.')
return
# Make sure the values are floats and make sure it's really
# a column vector.
spin = [ float(spin[0]), float(spin[1]), float(spin[2]) ]
spin = Matrix(spin)
# Make sure its normalized. Do this silently, since it will
# probably happen most of the time due to imprecision.
if spin.norm() != 1.0: spin = spin/spin.norm()
# Calculate the measurement operator as a weighted combination
# of the three Pauli matrices.
O_1 = spin[0]*sigma_x + spin[1]*sigma_y + spin[2]*sigma_z
O_1.simplify()
# the eigenvectors will be the two possible states. The
# "minus" state will be the first one.
evals, evecs = find_eigenvectors(O_1)
plus_state = evecs[1]
minus_state = evecs[0]
return(spin, plus_state, minus_state)
def getj_0j_1():
a1, a2 = dynamicsymbols('a1, a2')
mass_matrix = kane.mass_matrix
forcing = kane.forcing
tf_0 = Matrix([[cos(theta1), -sin(theta1), -leg_length*sin(theta1)], [sin(theta1), cos(theta1), leg_length*cos(theta1)], [0,0,1]])
tf_1 = Matrix([[cos(theta2), -sin(theta2), -body_length*sin(theta2)], [sin(theta2), cos(theta2), body_length*cos(theta2)], [0,0,1]])
tf_01 = simplify(tf_0*tf_1)
com_0 = Matrix([-leg_length*sin(theta1), leg_length*cos(theta1), 1])
com_1 = Matrix([-body_length*sin(theta2), body_length*cos(theta2), 1])
com_01 = simplify(tf_0*com_1)
com = Matrix(com_0 + com_01)
j_0 = com_0.jacobian([theta1, theta2])
j_1 = simplify(com_01.jacobian([theta1, theta2]))
j = com.jacobian([theta1, theta2])
q = Matrix(coordinates)
qdot = Matrix(speeds)
qddot = Matrix([a1, a2])
return [j_0, j_1]
def main():
u, v, R = symbols('u v R', real=True)
xi, eta = symbols(r'\xi \eta', cls=Function)
numer = 4*R**2
denom = u**2 + v**2 + numer
# inverse of a stereographic projection from the south pole
# onto the XY plane:
pinv = Matrix([numer * u / denom,
numer * v / denom,
-(2 * R * (u**2 + v**2)) / denom]) # OK
if False:
# textbook style
Dpinv = simplify(pinv.jacobian([u, v]))
print_latex(Dpinv, mat_str='pmatrix', mat_delim=None) # OK?
tDpinvDpinv = factor(Dpinv.transpose() @ Dpinv)
print_latex(tDpinvDpinv, mat_str='pmatrix', mat_delim=None) # OK
tDpinvDpinv = tDpinvDpinv.subs([(u, xi(t)), (v, eta(t))])
dcdt = Matrix([xi(t).diff(), eta(t).diff()])
print_latex(simplify(
sqrt((dcdt.transpose() @ tDpinvDpinv).dot(dcdt))))
else:
# directly
dpinvc = pinv.subs([(u, xi(t)), (v, eta(t))]).diff(t, 1)
print_latex(sqrt(factor(dpinvc.dot(dpinvc))))
def automaton_growth_func( automaton, initial_state, variable='z' ):
"""Returns Z-transform of the growth function of this automaton
Uses SYmpy for calculations"""
from sympy import Symbol, Matrix, eye, cancel
z = variable if isinstance(variable,Symbol) else Symbol(variable)
n = automaton.num_states()
a_,b_,c_ = growth_matrices(automaton, initial_state)
#a.print_nonzero()
c = Matrix( 1, n, c_)
b = Matrix( n, 1, b_)
a = Matrix( a_)
del a_, b_, c_
#(z*eye(n)-a).inv().print_nonzero()
if False: #naiive implementation
q = z*eye(n)-a
f = c * q.LUsolve(b) * z
assert f.shape == (1,1)
return f[0,0]
if True:
#use trick...
den = a.berkowitz_charpoly(z)
#print("#### charpoly:", den)
num = (a - b*c).berkowitz_charpoly(z) - den
#print("#### numerator:", num)
return num/den
def MatrizInversa(a, b,lista):
ma = json.loads(a)
mb = json.loads(b)
A = Matrix(ma)
B = Matrix(mb)
try:
A_inv = A.inv()
except ValueError:
lista.append('\n')
lista.append( 'Matriz es singular, no se puede resolver!')
lista.append('\n')
else:
ans = A_inv*B
MA = A.tolist()
MB = B.tolist()
def compute_lambda(robo, symo, j, antRj, antPj, lam):
"""Internal function. Computes the inertia parameters
transformation matrix
Notes
=====
lam is the output paramete
"""
lamJJ_list = []
lamJMS_list = []
for e1 in xrange(3):
for e2 in xrange(e1, 3):
u = vec_mut_J(antRj[j][:, e1], antRj[j][:, e2])
if e1 != e2:
u += vec_mut_J(antRj[j][:, e2], antRj[j][:, e1])
lamJJ_list.append(u.T)
for e1 in xrange(3):
v = vec_mut_MS(antRj[j][:, e1], antPj[j])
lamJMS_list.append(v.T)
lamJJ = Matrix(lamJJ_list).T # , 'LamJ', j)
lamJMS = symo.mat_replace(Matrix(lamJMS_list).T, 'LamMS', j)
lamJM = symo.mat_replace(vec_mut_M(antPj[j]), 'LamM', j)
lamJ = lamJJ.row_join(lamJMS).row_join(lamJM)
lamMS = sympy.zeros(3, 6).row_join(antRj[j]).row_join(antPj[j])
lamM = sympy.zeros(1, 10)
lamM[9] = 1
lam[j] = Matrix([lamJ, lamMS, lamM])
def etape(self, frontier, i_graph):
""" Calculates the most probable seed within the infected nodes"""
# Taking the actual submatrix, not the laplacian matrix. The change
# lies in the total number of connections (The diagonal terms) for the
# infected nodes connected to uninfected ones in the initial graph
i_laplacian_matrix = nx.laplacian_matrix(i_graph)
for i in range(0, len(i_graph.nodes())):
if frontier.has_node(i_graph.nodes()[i]):
i_laplacian_matrix[i, i] +=\
frontier.node[i_graph.nodes()[i]]['clear']
# SymPy
Lm = Matrix(i_laplacian_matrix.todense())
i = self.Sym2NumArray(Matrix(Lm.eigenvects()[0][2][0])).argmax()
# NumPy
# val, vect = linalg.eigh(i_laplacian_matrix.todense())
#i = vect[0].argmax()
# SciPY
# val, vect = eigs(i_laplacian_matrix.rint())
# i = vect[:, 0].argmax()
seed = (i_graph.nodes()[i])
return seed
def printnp(m):
"""
Prints a sympy matrix in the same format as a numpy array
"""
m = Matrix(m)
if m.shape[1] == 1:
m = m.reshape(1, m.shape[0])
elem_len = max(num_chars(d) for d in m.vec())
if m.shape[0] > 1:
outstr = '[['
else:
outstr = '['
for i in xrange(m.shape[0]):
if i:
outstr += ' ['
char_count = 0
for j, elem in enumerate(m[i, :]):
char_count += elem_len
if char_count > 77:
outstr += '\n '
char_count = elem_len + 2
# Add spaces
outstr += ' ' * (elem_len - num_chars(elem) +
int(j != 0)) + str(elem)
if i < m.shape[0] - 1:
outstr += ']\n'
if m.shape[0] > 1:
outstr += ']]'
else:
outstr += ']'
print outstr
def jacobian_dHdV(nK, y, cap, inds):
# Set up equations of power flow (power at line from nodal voltage) as symbolic equations and
# calculate the corresponding Jacobian matrix.
from sympy import symbols, Matrix
g = Matrix(np.real(y))
b = Matrix(np.imag(y))
e_J = symbols("e1:%d"%(nK+1))
f_J = symbols("f1:%d"%(nK+1))
hSluV = []
if "Pl" in inds:
nPl = np.asarray(inds["Pl"]).astype(int)
for k in range(nPl.shape[0]):
i,j = nPl[k,:]
hSluV.append((e_J[i]**2+f_J[i]**2)*g[i,j]-(e_J[i]*e_J[j]+f_J[i]*f_J[j])*g[i,j]+(e_J[i]*f_J[j]-e_J[j]*f_J[i])*b[i,j])
if "Ql" in inds:
nQl = np.asarray(inds["Ql"]).astype(int)
for k in range(nQl.shape[0]):
i,j = nQl[k,:]
hSluV.append(-(e_J[i]**2+f_J[i]**2)*b[i,j]+(e_J[i]*e_J[j]+f_J[i]*f_J[j])*b[i,j]+(e_J[i]*f_J[j]-e_J[j]*f_J[i])*g[i,j]-(e_J[i]**2+f_J[i]**2)*cap[i,j]/2)
if "Vm" in inds:
nVk = np.asarray(inds["Vm"]).astype(int)
for k in range(nVk.shape[0]):
i = nVk[k]
hSluV.append((e_J[i]**2+f_J[i]**2)**0.5)
if len(hSluV)>0:
hSluV = Matrix(hSluV)
ef_J = e_J[1:] + f_J[1:]
JMatrix_dHdV = hSluV.jacobian(ef_J)
return JMatrix_dHdV, hSluV
else:
return None, None
def _initialize_vectors(self, q_ind, q_dep, u_ind, u_dep, u_aux):
"""Initialize the coordinate and speed vectors."""
none_handler = lambda x: Matrix(x) if x else Matrix()
# Initialize generalized coordinates
q_dep = none_handler(q_dep)
if not iterable(q_ind):
raise TypeError('Generalized coordinates must be an iterable.')
if not iterable(q_dep):
raise TypeError('Dependent coordinates must be an iterable.')
q_ind = Matrix(q_ind)
self._qdep = q_dep
self._q = Matrix([q_ind, q_dep])
self._qdot = self._q.diff(dynamicsymbols._t)
# Initialize generalized speeds
u_dep = none_handler(u_dep)
if not iterable(u_ind):
raise TypeError('Generalized speeds must be an iterable.')
if not iterable(u_dep):
raise TypeError('Dependent speeds must be an iterable.')
u_ind = Matrix(u_ind)
self._udep = u_dep
self._u = Matrix([u_ind, u_dep])
self._udot = self._u.diff(dynamicsymbols._t)
self._uaux = none_handler(u_aux)
def test_matrix_tensor_product():
l1 = zeros(4)
for i in range(16):
l1[i] = 2**i
l2 = zeros(4)
for i in range(16):
l2[i] = i
l3 = zeros(2)
for i in range(4):
l3[i] = i
vec = Matrix([1,2,3])
#test for Matrix known 4x4 matricies
numpyl1 = np.matrix(l1.tolist())
numpyl2 = np.matrix(l2.tolist())
numpy_product = np.kron(numpyl1,numpyl2)
args = [l1, l2]
sympy_product = matrix_tensor_product(*args)
assert numpy_product.tolist() == sympy_product.tolist()
numpy_product = np.kron(numpyl2,numpyl1)
args = [l2, l1]
sympy_product = matrix_tensor_product(*args)
assert numpy_product.tolist() == sympy_product.tolist()
#test for other known matrix of different dimensions
numpyl2 = np.matrix(l3.tolist())
numpy_product = np.kron(numpyl1,numpyl2)
args = [l1, l3]
sympy_product = matrix_tensor_product(*args)
assert numpy_product.tolist() == sympy_product.tolist()
numpy_product = np.kron(numpyl2,numpyl1)
args = [l3, l1]
sympy_product = matrix_tensor_product(*args)
assert numpy_product.tolist() == sympy_product.tolist()
#test for non square matrix
numpyl2 = np.matrix(vec.tolist())
numpy_product = np.kron(numpyl1,numpyl2)
args = [l1, vec]
sympy_product = matrix_tensor_product(*args)
assert numpy_product.tolist() == sympy_product.tolist()
numpy_product = np.kron(numpyl2,numpyl1)
args = [vec, l1]
sympy_product = matrix_tensor_product(*args)
assert numpy_product.tolist() == sympy_product.tolist()
#test for random matrix with random values that are floats
random_matrix1 = np.random.rand(np.random.rand()*5+1,np.random.rand()*5+1)
random_matrix2 = np.random.rand(np.random.rand()*5+1,np.random.rand()*5+1)
numpy_product = np.kron(random_matrix1,random_matrix2)
args = [Matrix(random_matrix1.tolist()),Matrix(random_matrix2.tolist())]
sympy_product = matrix_tensor_product(*args)
assert not (sympy_product - Matrix(numpy_product.tolist())).tolist() > \
(ones((sympy_product.rows,sympy_product.cols))*epsilon).tolist()
#test for three matrix kronecker
sympy_product = matrix_tensor_product(l1,vec,l2)
numpy_product = np.kron(l1,np.kron(vec,l2))
assert numpy_product.tolist() == sympy_product.tolist()
def calculate_dependencies(equations, task_variables, variables_values):
replacements = replace.get_replacements(task_variables)[1]
transformed_equations = replace.get_transformed_expressions(equations, task_variables)
#prepare data for Numpy manipulations - change variables names to the names of special format - 'x%sy', also
#convert some strings to Symbols so they could be processed by Sympy and Numpy libs
symbolized_replaced_task_variables = sorted(symbols(replacements.keys()))
replaced_variables_values = {}
for k,v in replacements.iteritems():
replaced_variables_values[Symbol(k)] = variables_values[v]
#prepare the system of equations for solving - find Jacobian, in Jacobian matrix that was found replace variables by their values
F = Matrix(transformed_equations)
J = F.jacobian(symbolized_replaced_task_variables).evalf(subs=replaced_variables_values)
X = np.transpose(np.array(J))
#solve the system
n,m = X.shape
if (n != m - 1):
# if the system of equations is overdetermined (the number of equations is bigger than number of variables
#then we have to make regular, square systems of equations out of it (we do that by grabbing only some of the equations)
square_systems = suggest_square_systems_list(X, n, m)
for sq in square_systems:
try:
solve_system(sq, equations)
except Exception as e:
print e.args
print sq
else:
solve_system(X, equations)
def jacobian(functions, variables, constants):
symbols = dict((v, Symbol("x[%d]" % i)) for (i, v) in enumerate(variables))
f = Matrix([parse_func(expr, symbols, constants) for expr in functions])
x = Matrix(list(symbols.values()))
return lambdify("x", f), lambdify("x", f.jacobian(x))
#
# $$Q = G\cdot G^T \cdot \sigma_a^2$$
#
# with $G = \begin{bmatrix}0.5dt^2 & 0.5dt^2 & dt & dt & 1.0 & 1.0\end{bmatrix}^T$ and $\sigma_a$ as the acceleration process noise.
# <headingcell level=4>
# Symbolic Calculation
# <codecell>
from sympy import Symbol, Matrix
from sympy.interactive import printing
printing.init_printing()
dts = Symbol('\Delta t')
Qs = Matrix([[0.5*dts**2],[0.5*dts**2],[dts],[dts],[1.0],[1.0]])
Qs*Qs.T
# <codecell>
sa = 0.001
G = np.matrix([[1/2.0*dt**2],
[1/2.0*dt**2],
[dt],
[dt],
[1.0],
[1.0]])
Q = G*G.T*sa**2
print(Q, Q.shape)
# Primary State Rates
x_dot = v * cos(theta)
y_dot = v * sin(theta)
theta_dot = u_max * sin(w)
writeList = [x_dot, y_dot, theta_dot]
# Covariance Calculations
p11, p12, \
p22, \
= symbols('p11 p12 \
p22' )
P = Matrix([[p11, p12], [p12, p22]])
F = Matrix([[diff(x_dot, x), diff(x_dot, y)],
[diff(y_dot, x), diff(y_dot, y)]])
G = Matrix([[cos(theta)], [sin(theta)]])
h = sqrt((x - xb)**2 + (y - yb)**2)
H = Matrix([[diff(h, x), diff(h, y)]])
Q = Dt * diag(sigv**2)
R = Dt * diag(sigr**2)
P_dot = (F * P + P * F.T - P * H.T * (R**-1) * H * P + G * Q * G.T)
import sympy
from sympy.abc import alpha, x, y, v, w, R, theta
from sympy import symbols, Matrix
from numpy import array
import numpy as np
# px, py = symbols('p_x, p_y')
# # h of landmark: (x,y,yaw)-->(range,bearing)
# z = Matrix([[sympy.sqrt((px-x)**2 + (py-y)**2)],
# [sympy.atan2(py-y, px-x) - theta]])
# # H jacobian
# print(z.jacobian(Matrix([x, y, theta])))
# 定义符号
a, x, y, v, w, theta, time = symbols('a, x, y, v, w, theta, t')
d = v * time # km
beta = (d / w) * sympy.tan(a) # rad
r = w / sympy.tan(a) # km
r_lo = r / 111 / sympy.cos(y / 180 * np.pi)
r_la = r / 111
# 定义predict转移函数h形式，单位
fxu = Matrix([[x - r_lo * sympy.sin(theta) + r_lo * sympy.sin(theta + beta)],
[y + r_la * sympy.cos(theta) - r_la * sympy.cos(theta + beta)],
[theta + beta]])
print(fxu)
"r"] #l=v_l, r=v_r
# Differential drive approximation
# Velocity calculations
v_k = 0.5 * wheel_rad * (l + r)
x_dot_k = v_k * cos(phi)
y_dot_k = v_k * sin(phi)
phi_dot_k = (wheel_rad / wheelbase_len) * (l - r)
# Position calculations
x_k = x + x_dot * t
y_k = y + y_dot * t
phi_k = phi + phi_dot * t
# Setup vector of the functions
f = Matrix([x_k, x_dot_k, y_k, y_dot_k, phi_k, phi_dot_k, l, r])
# Setup vector of the variables
X = Matrix([x, x_dot, y, y_dot, phi, phi_dot, l, r])
# IGVC Fk matrix
model = f
jac_model = f.jacobian(X)
# # Input dictionary setup
# input_dict = {self.state_vars[i]: last_xk[i] for i in range(0, len(self.state_vars))}
# input_dict['t'] = dt
# ctrl_dict = {self.ctrl_vars[i]: ctrl[i] for i in range(0, len(self.ctrl_vars))}
# F = self.jac_model.subs(input_dict).subs(ctrl_dict)
# new_state = self.model.subs(input_dict).subs(ctrl_dict)
alpha_dot = u_max * sin(u)
writeList = [h_dot, theta_dot, v_dot, gam_dot, alpha_dot]
# Covariance Calculations
p11, p12, p13, p14, \
p22, p23, p24, \
p33, p34, \
p44 \
= symbols('p11 p12 p13 p14 \
p22 p23 p24 \
p33 p34 \
p44' )
P = Matrix([[p11, p12, p13, p14], [p12, p22, p23, p24],
[p13, p23, p33, p34], [p14, p24, p34, p44]])
F = Matrix([[
diff(h_dot, h),
diff(theta_dot, h),
diff(v_dot, h),
diff(gam_dot, h)
],
[
diff(h_dot, theta),
diff(theta_dot, theta),
diff(v_dot, theta),
diff(gam_dot, theta)
],
[
diff(h_dot, v),
B = A.rotate('B', 2, a)
# C rotates about an axis along the tangent to the center of the circular cross
# section by an angle b
C = B.rotate('C', 3, b)
# Locate an arbitrary point on the torus
p = Vector(r1*B[1] + c*C[2])
# Express it in the A reference frame
x = dot(p, A[1])
y = dot(p, A[2])
z = dot(p, A[3])
# Determinant of the Jacobian of the mapping from a, b, c to x, y, z
X = [a, b, c]
F = Matrix([x, y, z])
J = F.jacobian(X)
dv = (J.det(method='berkowitz')).expand().subs({cos(a)**2:1-sin(a)**2,
cos(b)**2:1-sin(b)**2}).expand()
print 'dx*dy*dz =(', dv, ')*da*db*dc'
# Integrands of definition of inertia scalars
i11 = rho * dot(cross(p, A[1]), cross(p, A[1])) * dv
i22 = rho * dot(cross(p, A[2]), cross(p, A[2])) * dv
i33 = rho * dot(cross(p, A[3]), cross(p, A[3])) * dv
i12 = rho * dot(cross(p, A[1]), cross(p, A[2])) * dv
i13 = rho * dot(cross(p, A[1]), cross(p, A[3])) * dv
i23 = rho * dot(cross(p, A[2]), cross(p, A[3])) * dv
I11 = Int(Int(Int(i11, (a, 0, 2*pi)), (b, 0, 2*pi)), (c, 0, r2))
Pos_scaled[0, i] = scale(Pos_origin[i, 0], True)
Pos_scaled[1, i] = scale(Pos_origin[i, 1], False)
update_points()
print("init: ready")
# sympy
q = MatrixSymbol('q', 1, low_dim) # updated position
p_i = MatrixSymbol('p_i', 1, high_dim) # selected point (high_dim) (P[thisID])
E = MatrixSymbol('E', high_dim, high_dim) # = (e1 e2 e0 0 ...) : 縦ベクトルの列
A_inputEs = MatrixSymbol('A_inputEs', low_dim + 1, low_dim) # ei' の係数行列
A_inputRs = MatrixSymbol('A_inputRs', low_dim, low_dim - 1) # Ri の係数行列
var = (q, p_i, E, A_inputEs, A_inputRs) # 変数のリスト
A = Matrix(np.zeros(high_dim * high_dim).reshape(high_dim, high_dim))
A[0:low_dim + 1, 0:low_dim] = Matrix(A_inputEs)
A[0:low_dim, low_dim:low_dim + 1] = Matrix(A_inputRs)
_E = E * A # = (e1' e2' R1 0 ...) : 更新後の射影ベクトルは縦ベクトルで格納
# 制約解消における前計算
constraints1 = (refine(_E.T * _E, Q.orthogonal(E))).doit()
constraints2 = (refine(E.T * _E, Q.orthogonal(E))).doit()
constraints3 = p_i * _E
# ei' * ej' = δij (クロネッカーのデルタ)
bases_e = Matrix(constraints1[0:low_dim, 0:low_dim] -
Matrix(Identity(low_dim)))
_bases_e = bases_e[0, 0]**2 + bases_e[0, 1]**2 + bases_e[1, 1]**2
def inertia_matrix(dyadic, rf):
"""Return the inertia matrix of a given dyadic for a specified
reference frame.
"""
return Matrix([[dot(dot(dyadic, i), j) for j in rf] for i in rf])
def test_simplify_expr():
x, y, z, k, n, m, w, s, A = symbols('x,y,z,k,n,m,w,s,A')
f = Function('f')
assert all(simplify(tmp) == tmp for tmp in [I, E, oo, x, -x, -oo, -E, -I])
e = 1 / x + 1 / y
assert e != (x + y) / (x * y)
assert simplify(e) == (x + y) / (x * y)
e = A**2 * s**4 / (4 * pi * k * m**3)
assert simplify(e) == e
e = (4 + 4 * x - 2 * (2 + 2 * x)) / (2 + 2 * x)
assert simplify(e) == 0
e = (-4 * x * y**2 - 2 * y**3 - 2 * x**2 * y) / (x + y)**2
assert simplify(e) == -2 * y
e = -x - y - (x + y)**(-1) * y**2 + (x + y)**(-1) * x**2
assert simplify(e) == -2 * y
e = (x + x * y) / x
assert simplify(e) == 1 + y
e = (f(x) + y * f(x)) / f(x)
assert simplify(e) == 1 + y
e = (2 * (1 / n - cos(n * pi) / n)) / pi
assert simplify(e) == (-cos(pi * n) + 1) / (pi * n) * 2
e = integrate(1 / (x**3 + 1), x).diff(x)
assert simplify(e) == 1 / (x**3 + 1)
e = integrate(x / (x**2 + 3 * x + 1), x).diff(x)
assert simplify(e) == x / (x**2 + 3 * x + 1)
f = Symbol('f')
A = Matrix([[2 * k - m * w**2, -k], [-k, k - m * w**2]]).inv()
assert simplify((A * Matrix([0, f]))[1] - (-f * (2 * k - m * w**2) /
(k**2 - (k - m * w**2) *
(2 * k - m * w**2)))) == 0
f = -x + y / (z + t) + z * x / (z + t) + z * a / (z + t) + t * x / (z + t)
assert simplify(f) == (y + a * z) / (z + t)
# issue 10347
expr = -x * (y**2 - 1) * (
2 * y**2 * (x**2 - 1) / (a * (x**2 - y**2)**2) + (x**2 - 1) /
(a * (x**2 - y**2))) / (a * (x**2 - y**2)) + x * (
-2 * x**2 * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * sin(z) /
(a * (x**2 - y**2)**2) -
x**2 * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * sin(z) /
(a * (x**2 - 1) * (x**2 - y**2)) +
(x**2 * sqrt((-x**2 + 1) * (y**2 - 1)) *
sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * sin(z) / (x**2 - 1) + sqrt(
(-x**2 + 1) * (y**2 - 1)) *
(x * (-x * y**2 + x) / sqrt(-x**2 * y**2 + x**2 + y**2 - 1) +
sqrt(-x**2 * y**2 + x**2 + y**2 - 1)) * sin(z)) / (a * sqrt(
(-x**2 + 1) * (y**2 - 1)) * (x**2 - y**2))
) * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * sin(z) / (
a * (x**2 - y**2)) + x * (
-2 * x**2 * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * cos(z) /
(a * (x**2 - y**2)**2) -
x**2 * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * cos(z) /
(a * (x**2 - 1) * (x**2 - y**2)) +
(x**2 * sqrt((-x**2 + 1) *
(y**2 - 1)) * sqrt(-x**2 * y**2 + x**2 + y**2 - 1)
* cos(z) / (x**2 - 1) + x * sqrt(
(-x**2 + 1) * (y**2 - 1)) * (-x * y**2 + x) * cos(z) /
sqrt(-x**2 * y**2 + x**2 + y**2 - 1) + sqrt(
(-x**2 + 1) *
(y**2 - 1)) * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) *
cos(z)) / (a * sqrt((-x**2 + 1) * (y**2 - 1)) * (x**2 - y**2))
) * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * cos(z) / (
a *
(x**2 - y**2)) - y * sqrt((-x**2 + 1) * (y**2 - 1)) * (
-x * y * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * sin(z) /
(a * (x**2 - y**2) *
(y**2 - 1)) +
2 * x * y * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * sin(z) /
(a * (x**2 - y**2)**2) +
(x * y * sqrt((-x**2 + 1) * (y**2 - 1)) *
sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * sin(z) /
(y**2 - 1) + x * sqrt(
(-x**2 + 1) * (y**2 - 1)) * (-x**2 * y + y) * sin(z) /
sqrt(-x**2 * y**2 + x**2 + y**2 - 1)) / (a * sqrt(
(-x**2 + 1) * (y**2 - 1)) * (x**2 - y**2))
) * sin(z) / (a * (x**2 - y**2)) + y * (x**2 - 1) * (
-2 * x * y *
(x**2 - 1) /
(a * (x**2 - y**2)**2) + 2 * x * y / (a * (x**2 - y**2))
) / (a *
(x**2 - y**2)) + y * (x**2 - 1) * (y**2 - 1) * (
-x * y * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) *
cos(z) / (a * (x**2 - y**2) *
(y**2 - 1)) + 2 * x * y *
sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * cos(z) /
(a *
(x**2 - y**2)**2) +
(x * y * sqrt((-x**2 + 1) * (y**2 - 1)) *
sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * cos(z) /
(y**2 - 1) + x * sqrt(
(-x**2 + 1) * (y**2 - 1)) * (-x**2 * y + y) *
cos(z) / sqrt(-x**2 * y**2 + x**2 + y**2 - 1)) /
(a * sqrt((-x**2 + 1) * (y**2 - 1)) *
(x**2 - y**2))) * cos(z) / (a * sqrt(
(-x**2 + 1) *
(y**2 - 1)) * (x**2 - y**2)) - x * sqrt(
(-x**2 + 1) * (y**2 - 1)
) * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * sin(
z)**2 / (a**2 * (x**2 - 1) * (x**2 - y**2) *
(y**2 - 1)) - x * sqrt(
(-x**2 + 1) *
(y**2 - 1)) * sqrt(
-x**2 * y**2 + x**2 + y**2 -
1) * cos(z)**2 / (
a**2 * (x**2 - 1) *
(x**2 - y**2) *
(y**2 - 1))
assert simplify(expr) == 2 * x / (a**2 * (x**2 - y**2))
#issue 17631
assert simplify('((-1/2)*Boole(True)*Boole(False)-1)*Boole(True)') == \
Mul(sympify('(2 + Boole(True)*Boole(False))'), sympify('-Boole(True)/2'))
A, B = symbols('A,B', commutative=False)
assert simplify(A * B - B * A) == A * B - B * A
assert simplify(A / (1 + y / x)) == x * A / (x + y)
assert simplify(A * (1 / x + 1 / y)) == A / x + A / y #(x + y)*A/(x*y)
assert simplify(log(2) + log(3)) == log(6)
assert simplify(log(2 * x) - log(2)) == log(x)
assert simplify(hyper([], [], x)) == exp(x)
def test_issue_10567():
a, b, c, t = symbols('a b c t')
vt = Matrix([a*t, b, c])
assert integrate(vt, t) == Integral(vt, t).doit()
assert integrate(vt, t) == Matrix([[a*t**2/2], [b*t], [c*t]])
def test_aux_dep():
# This test is about rolling disc dynamics, comparing the results found
# with KanesMethod to those found when deriving the equations "manually"
# with SymPy.
# The terms Fr, Fr*, and Fr*_steady are all compared between the two
# methods. Here, Fr*_steady refers to the generalized inertia forces for an
# equilibrium configuration.
# Note: comparing to the test of test_rolling_disc() in test_kane.py, this
# test also tests auxiliary speeds and configuration and motion constraints
#, seen in the generalized dependent coordinates q[3], and depend speeds
# u[3], u[4] and u[5].
# First, mannual derivation of Fr, Fr_star, Fr_star_steady.
# Symbols for time and constant parameters.
# Symbols for contact forces: Fx, Fy, Fz.
t, r, m, g, I, J = symbols('t r m g I J')
Fx, Fy, Fz = symbols('Fx Fy Fz')
# Configuration variables and their time derivatives:
# q[0] -- yaw
# q[1] -- lean
# q[2] -- spin
# q[3] -- dot(-r*B.z, A.z) -- distance from ground plane to disc center in
# A.z direction
# Generalized speeds and their time derivatives:
# u[0] -- disc angular velocity component, disc fixed x direction
# u[1] -- disc angular velocity component, disc fixed y direction
# u[2] -- disc angular velocity component, disc fixed z direction
# u[3] -- disc velocity component, A.x direction
# u[4] -- disc velocity component, A.y direction
# u[5] -- disc velocity component, A.z direction
# Auxiliary generalized speeds:
# ua[0] -- contact point auxiliary generalized speed, A.x direction
# ua[1] -- contact point auxiliary generalized speed, A.y direction
# ua[2] -- contact point auxiliary generalized speed, A.z direction
q = dynamicsymbols('q:4')
qd = [qi.diff(t) for qi in q]
u = dynamicsymbols('u:6')
ud = [ui.diff(t) for ui in u]
ud_zero = dict(zip(ud, [0.]*len(ud)))
ua = dynamicsymbols('ua:3')
ua_zero = dict(zip(ua, [0.]*len(ua)))
# Reference frames:
# Yaw intermediate frame: A.
# Lean intermediate frame: B.
# Disc fixed frame: C.
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q[0], N.z])
B = A.orientnew('B', 'Axis', [q[1], A.x])
C = B.orientnew('C', 'Axis', [q[2], B.y])
# Angular velocity and angular acceleration of disc fixed frame
# u[0], u[1] and u[2] are generalized independent speeds.
C.set_ang_vel(N, u[0]*B.x + u[1]*B.y + u[2]*B.z)
C.set_ang_acc(N, C.ang_vel_in(N).diff(t, B)
+ cross(B.ang_vel_in(N), C.ang_vel_in(N)))
# Velocity and acceleration of points:
# Disc-ground contact point: P.
# Center of disc: O, defined from point P with depend coordinate: q[3]
# u[3], u[4] and u[5] are generalized dependent speeds.
P = Point('P')
P.set_vel(N, ua[0]*A.x + ua[1]*A.y + ua[2]*A.z)
O = P.locatenew('O', q[3]*A.z + r*sin(q[1])*A.y)
O.set_vel(N, u[3]*A.x + u[4]*A.y + u[5]*A.z)
O.set_acc(N, O.vel(N).diff(t, A) + cross(A.ang_vel_in(N), O.vel(N)))
# Kinematic differential equations:
# Two equalities: one is w_c_n_qd = C.ang_vel_in(N) in three coordinates
# directions of B, for qd0, qd1 and qd2.
# the other is v_o_n_qd = O.vel(N) in A.z direction for qd3.
# Then, solve for dq/dt's in terms of u's: qd_kd.
w_c_n_qd = qd[0]*A.z + qd[1]*B.x + qd[2]*B.y
v_o_n_qd = O.pos_from(P).diff(t, A) + cross(A.ang_vel_in(N), O.pos_from(P))
kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
[dot(v_o_n_qd - O.vel(N), A.z)])
qd_kd = solve(kindiffs, qd)
# Values of generalized speeds during a steady turn for later substitution
# into the Fr_star_steady.
steady_conditions = solve(kindiffs.subs({qd[1] : 0, qd[3] : 0}), u)
steady_conditions.update({qd[1] : 0, qd[3] : 0})
# Partial angular velocities and velocities.
partial_w_C = [C.ang_vel_in(N).diff(ui, N) for ui in u + ua]
partial_v_O = [O.vel(N).diff(ui, N) for ui in u + ua]
partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua]
# Configuration constraint: f_c, the projection of radius r in A.z direction
# is q[3].
# Velocity constraints: f_v, for u3, u4 and u5.
# Acceleration constraints: f_a.
f_c = Matrix([dot(-r*B.z, A.z) - q[3]])
f_v = Matrix([dot(O.vel(N) - (P.vel(N) + cross(C.ang_vel_in(N),
O.pos_from(P))), ai).expand() for ai in A])
v_o_n = cross(C.ang_vel_in(N), O.pos_from(P))
a_o_n = v_o_n.diff(t, A) + cross(A.ang_vel_in(N), v_o_n)
f_a = Matrix([dot(O.acc(N) - a_o_n, ai) for ai in A])
# Solve for constraint equations in the form of
# u_dependent = A_rs * [u_i; u_aux].
# First, obtain constraint coefficient matrix: M_v * [u; ua] = 0;
# Second, taking u[0], u[1], u[2] as independent,
# taking u[3], u[4], u[5] as dependent,
# rearranging the matrix of M_v to be A_rs for u_dependent.
# Third, u_aux ==0 for u_dep, and resulting dictionary of u_dep_dict.
M_v = zeros(3, 9)
for i in range(3):
for j, ui in enumerate(u + ua):
M_v[i, j] = f_v[i].diff(ui)
M_v_i = M_v[:, :3]
M_v_d = M_v[:, 3:6]
M_v_aux = M_v[:, 6:]
M_v_i_aux = M_v_i.row_join(M_v_aux)
A_rs = - M_v_d.inv() * M_v_i_aux
u_dep = A_rs[:, :3] * Matrix(u[:3])
u_dep_dict = dict(zip(u[3:], u_dep))
# Active forces: F_O acting on point O; F_P acting on point P.
# Generalized active forces (unconstrained): Fr_u = F_point * pv_point.
F_O = m*g*A.z
F_P = Fx * A.x + Fy * A.y + Fz * A.z
Fr_u = Matrix([dot(F_O, pv_o) + dot(F_P, pv_p) for pv_o, pv_p in
zip(partial_v_O, partial_v_P)])
# Inertia force: R_star_O.
# Inertia of disc: I_C_O, where J is a inertia component about principal axis.
# Inertia torque: T_star_C.
# Generalized inertia forces (unconstrained): Fr_star_u.
R_star_O = -m*O.acc(N)
I_C_O = inertia(B, I, J, I)
T_star_C = -(dot(I_C_O, C.ang_acc_in(N)) \
+ cross(C.ang_vel_in(N), dot(I_C_O, C.ang_vel_in(N))))
Fr_star_u = Matrix([dot(R_star_O, pv) + dot(T_star_C, pav) for pv, pav in
zip(partial_v_O, partial_w_C)])
# Form nonholonomic Fr: Fr_c, and nonholonomic Fr_star: Fr_star_c.
# Also, nonholonomic Fr_star in steady turning condition: Fr_star_steady.
Fr_c = Fr_u[:3, :].col_join(Fr_u[6:, :]) + A_rs.T * Fr_u[3:6, :]
Fr_star_c = Fr_star_u[:3, :].col_join(Fr_star_u[6:, :])\
+ A_rs.T * Fr_star_u[3:6, :]
Fr_star_steady = Fr_star_c.subs(ud_zero).subs(u_dep_dict)\
.subs(steady_conditions).subs({q[3]: -r*cos(q[1])}).expand()
# Second, using KaneMethod in mechanics for fr, frstar and frstar_steady.
# Rigid Bodies: disc, with inertia I_C_O.
iner_tuple = (I_C_O, O)
disc = RigidBody('disc', O, C, m, iner_tuple)
bodyList = [disc]
# Generalized forces: Gravity: F_o; Auxiliary forces: F_p.
F_o = (O, F_O)
F_p = (P, F_P)
forceList = [F_o, F_p]
# KanesMethod.
kane = KanesMethod(
N, q_ind= q[:3], u_ind= u[:3], kd_eqs=kindiffs,
q_dependent=q[3:], configuration_constraints = f_c,
u_dependent=u[3:], velocity_constraints= f_v,
u_auxiliary=ua
)
# fr, frstar, frstar_steady and kdd(kinematic differential equations).
(fr, frstar)= kane.kanes_equations(forceList, bodyList)
frstar_steady = frstar.subs(ud_zero).subs(u_dep_dict).subs(steady_conditions)\
.subs({q[3]: -r*cos(q[1])}).expand()
kdd = kane.kindiffdict()
assert Matrix(Fr_c).expand() == fr.expand()
assert Matrix(Fr_star_c.subs(kdd)).expand() == frstar.expand()
assert (simplify(Matrix(Fr_star_steady).expand()) ==
simplify(frstar_steady.expand()))
def __init__(self,
site_species,
site_species_energies,
bond_energies,
intracluster_connections,
intercluster_connections,
compositional_interactions=None):
self.site_species = site_species
self.site_species_energies = site_species_energies
self.bond_energies = bond_energies
self.intracluster_connections = intracluster_connections
self.intercluster_connections = intercluster_connections
self.n_species_per_site = np.array([len(s) for s in site_species])
self.site_start_indices = (np.cumsum(self.n_species_per_site) -
self.n_species_per_site[0])
self.site_index_tuples = np.array(
[self.site_start_indices, self.n_species_per_site]).T
site_bounds = [0]
site_bounds.extend(list(np.cumsum(self.n_species_per_site)))
self.site_bounds = np.array([site_bounds[:-1], site_bounds[1:]]).T
self.n_sites = len(self.n_species_per_site)
self.n_site_species = np.sum(self.n_species_per_site)
self.n_ind = self.n_site_species - self.n_sites + 1
if not len(site_species) == len(self.n_species_per_site):
raise Exception('site_species must be a list of lists, '
'with the number of outermost lists equal to '
'the number of sites.')
if not all([
len(s) == self.n_species_per_site[i]
for i, s in enumerate(site_species)
]):
raise Exception('site_species must have the correct '
'number of species on each site')
self.site_species = site_species
self.site_species_flat = [
species for site in site_species for species in site
]
self.components = sorted(list(set(self.site_species_flat)))
self.n_components = len(self.components)
if compositional_interactions is None:
compositional_interactions = np.zeros(
(self.n_components, self.n_components))
# Make correlation matrix between composition and site species
self.site_species_compositions = np.zeros(
(len(self.site_species_flat), len(self.components)), dtype='int')
for i, ss in enumerate(self.site_species_flat):
self.site_species_compositions[i, self.components.index(ss)] = 1
clusters = product(*[
np.identity(n_species, dtype='int')
for n_species in self.n_species_per_site
])
self.n_clusters = np.prod(self.n_species_per_site)
self.cluster_occupancies = np.array([np.hstack(cl) for cl in clusters])
self.cluster_compositions = np.einsum('ij, jk->ik',
self.cluster_occupancies,
self.site_species_compositions)
self.pivots_flat = list(
sorted(set([list(c).index(1)
for c in self.cluster_occupancies.T])))
ind_cl_occs = np.array(
[self.cluster_occupancies[p] for p in self.pivots_flat],
dtype='int')
ind_cl_comps = np.array(
[self.cluster_compositions[p] for p in self.pivots_flat],
dtype='int')
self.independent_cluster_occupancies = ind_cl_occs
self.independent_cluster_compositions = ind_cl_comps
self.independent_interactions = np.einsum('ij, lk, jk->il',
ind_cl_comps, ind_cl_comps,
compositional_interactions)
null = Matrix(self.independent_cluster_compositions.T).nullspace()
rxn_matrix = np.array([np.array(v).T[0] for v in null])
self.isochemical_reactions = rxn_matrix
self.n_reactions = len(rxn_matrix)
self._ps_to_p_ind = pinv(self.independent_cluster_occupancies.T)
A_ind_shape = list(self.n_species_per_site)
A_ind_shape.append(self.n_ind)
self.A_ind = lstsq(self.independent_cluster_occupancies.T,
self.cluster_occupancies.T,
rcond=None)[0].round(decimals=10).T
self._AA = np.einsum('ik, ij -> ijk', self.A_ind, self.A_ind)
self.pivots = np.argwhere(np.sum(np.abs(self.A_ind), axis=-1) == 1)
shp = np.concatenate(
(self.n_species_per_site, self.n_species_per_site))
self.cluster_identity_matrix = np.eye(self.n_clusters).reshape(shp)
self.cluster_ones = np.ones(self.n_species_per_site)
self.cluster_energies = (
np.einsum('i, ki', self.site_species_energies,
self.cluster_occupancies) +
np.einsum('ij, ij, ki, kj->k', self.bond_energies,
(self.intercluster_connections +
self.intracluster_connections),
self.cluster_occupancies, self.cluster_occupancies))
self.intracluster_energies = (
np.einsum('i, ki', self.site_species_energies,
self.cluster_occupancies) +
np.einsum('ij, ij, ki, kj->k', self.bond_energies,
self.intracluster_connections, self.cluster_occupancies,
self.cluster_occupancies))
self.cluster_interactions = np.einsum('ij, ij, kj->ki',
self.bond_energies,
self.intercluster_connections,
self.cluster_occupancies)
np.random.seed(seed=19)
std = np.std(self.cluster_energies)
delta = np.random.rand(len(self.cluster_energies)) * std * 1.e-10
self._delta_cluster_energies = delta
def test_represent():
assert represent(Jz) == hbar * Matrix([[1, 0], [0, -1]]) / 2
assert represent(Jz,
j=1) == hbar * Matrix([[1, 0, 0], [0, 0, 0], [0, 0, -1]])
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]
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])
assert (trigsimp(fr_star.subs(vc_map).subs(u3, 0)).doit().expand() ==
fr_star_expected.expand())
# 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)))
fr2, fr_star2 = km.kanes_equations(forces, bodies2)
assert (trigsimp(fr_star2.subs(vc_map).subs(u3, 0)).doit().expand() ==
fr_star_expected.expand())
def prefixed_matrix(prefix):
"""
Returns a matrix where each entry is of the for prefix___name.
"""
return Matrix(4, 4, [symbols(prefix + "___" + i) for i in matrix_names])
def perspective(g):
"""
Returns the Ren'Py projection matrix. This is a view into a 3d space
where (0, 0) is the top left corner (`w`/2, `h`/2) is the center, and
(`w`,`h`) is the bottom right, when the z coordinate is 0.
`w`, `h`
The width and height of the input plane, in pixels.
`n`
The distance of the near plane from the camera.
`p`
The distance of the 1:1 plane from the camera. This is where 1 pixel
is one coordinate unit.
`f`
The distance of the far plane from the camera.
"""
w, h, n, p, f = g.parameters('w h n p f')
offset = Matrix(4, 4, [
1.0,
0.0,
0.0,
-w / 2.0,
0.0,
1.0,
0.0,
-h / 2.0,
0.0,
0.0,
1.0,
-p,
0.0,
0.0,
0.0,
1.0,
])
projection = Matrix(4, 4, [
2.0 * p / w,
0.0,
0.0,
0.0,
0.0,
2.0 * p / h,
0.0,
0.0,
0.0,
0.0,
-(f + n) / (f - n),
-2 * f * n / (f - n),
0.0,
0.0,
-1.0,
0.0,
])
reverse_offset = Matrix(4, 4, [
w / 2.0,
0.0,
0.0,
w / 2.0,
0.0,
h / 2.0,
0.0,
h / 2.0,
0.0,
0.0,
1.0,
0.0,
0.0,
0.0,
0.0,
1.0,
])
g.matrix(reverse_offset * projection * offset)
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])
fr, fr_star = km.kanes_equations(forces, bodies)
assert (fr.expand() == fr_expected.expand())
assert (trigsimp(fr_star).expand() == fr_star_expected.expand())
class IKSolver:
"""Class for performing iterative inverse kinematics for the blueberry robot"""
#Note: Joint arrays are listed in order according to the following indexing:
#[0] = shoulder rotation, [0] = shoulder flexure, [0] = elbow flexure, [0] = wrist rotation, [4] = wrist flexure
#Joint Limits in radians as tuples of [0] = lower bound and [1] = upper bound
_joint_limits = [(-1.134, 1.833), (-2.182, 0.785), (-1.309, 1.658),
(-1.571, 1.396), (-1.396, 1.571)]
#Joints for which the servos were mounted in reverse (I.E. positive increase in input value equals radian decrease)
_rev_joints = [False, True, False, False, True]
#Home position for the robot (pointing straight up)
_home = [70, 130, 80, 95, 85]
#Neutral gripper position to avoid servo strain
_grip_neutral = 30
#Fine tuning parameters for iterative inverse kinematics
#---How many gradient descents to perform while looking for a solution
_maxIterations = 5
#---How far to move on each step of an interation
_stepSize = .01
#---How close to the goal the solution must be to be sufficient
_goalAccuracy = .05
#---How many steps for each gradient descent
_subIterations = 250
#Current joint angles of the robot
_current = [0, 0, 0, 0, 0]
#Homogeneous transforms representing each frame from the base of the robot
# (Directly under the center of the robot)
# to the gripper ee (1 cm below the deepest part of the gripper while open)
# used to calculate forward kinematics
_q1, _q2, _q3, _q4, _q5 = symbols('_q1 _q2 _q3 _q4 _q5')
_T1 = Matrix([[cos(_q1), -sin(_q1), 0, 0], [sin(_q1),
cos(_q1), 0, 0],
[0, 0, 1, 0.085], [0, 0, 0, 1]])
_T2 = Matrix([[cos(_q2), 0,
sin(_q2),
cos(_q2 + np.pi / 2) * 0.085], [0, 1, 0, 0],
[-sin(_q2), 0,
cos(_q2),
sin(_q2 + np.pi / 2) * 0.085], [0, 0, 0, 1]])
_T3 = Matrix([[cos(_q3), 0,
sin(_q3),
cos(_q3 + np.pi / 2) * 0.115], [0, 1, 0, 0],
[-sin(_q3), 0,
cos(_q3),
sin(_q3 + np.pi / 2) * 0.115], [0, 0, 0, 1]])
_T4 = Matrix([[cos(_q4), -sin(_q4), 0, 0], [sin(_q4),
cos(_q4), 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
_T5 = Matrix([[cos(_q5), 0,
sin(_q5),
cos(_q5 + np.pi / 2) * 0.06], [0, 1, 0, 0],
[-sin(_q5), 0,
cos(_q5),
sin(_q5 + np.pi / 2) * 0.06], [0, 0, 0, 1]])
_FK = _T1 * _T2 * _T3 * _T4 * _T5
def __init__(self, usbPort):
"""Initialize a connection to the robot with inverse kinematic control"""
#Connect to board via usb (must have BlueberryHardwareControl loaded on it)
self.ser = serial.Serial(usbPort, 9600, timeout=.5)
#Get the translation only portion of the forward kinematics (since we don't have full 6 DOF for translation and orientation)
justTranslation = self._FK.col(-1)
justTranslation.row_del(-1)
input = Matrix([self._q1, self._q2, self._q3, self._q4, self._q5])
#Get jacobian for gradient descent
self.translationJacobian = justTranslation.jacobian(input)
def getEE(self, q):
"""Get the homogeneous transform of the end effector in the base frame given an array of joint angles"""
substituteDict = {
self._q1: q[0],
self._q2: q[1],
self._q3: q[2],
self._q4: q[3],
self._q5: q[4]
}
return self._FK.evalf(subs=substituteDict)
def solveForEFTranslation(self, goalX):
"""Get the joint configuration that will get the robot's end effector to (or close to) the goal position.
Takes in a 3d vector as a goal position
Returns a vector of joint angles """
iterations = 0
goal = Matrix(goalX)
dx = Matrix([1, 1, 1])
bestDX = Matrix([999, 999, 999])
bestQ = []
#Iterate until you find an acceptable end effector position or hit the max iterations
while iterations < self._maxIterations and bestDX.norm(
) > self._goalAccuracy:
iterations += 1
#Initialize to a random initial configuration (avoid local minimums)
q = [
self._joint_limits[0][0] +
random.random() * self._joint_limits[0][1],
self._joint_limits[1][0] +
random.random() * self._joint_limits[1][1],
self._joint_limits[2][0] +
random.random() * self._joint_limits[2][1],
self._joint_limits[3][0] +
random.random() * self._joint_limits[3][1],
self._joint_limits[4][0] +
random.random() * self._joint_limits[4][1]
]
k = 0
#Perfrom gradient descent over the next subIteration number of steps
numNudges = 0
while k < self._subIterations:
k += 1
#Get the difference of the end effector from the goal position
x = self.getEE(q)
x = x.col(-1)
x.row_del(-1)
dx = goal - x
#Use the inverse (pseduo, as this isn't square) of the jacobian at the current joint configuration to get a
#change in q that lowers the difference of the end effector from the goal position
substituteDict = {
self._q1: q[0],
self._q2: q[1],
self._q3: q[2],
self._q4: q[3],
self._q5: q[4]
}
jacobOutput = self.translationJacobian.evalf(
subs=substituteDict)
dq = self._stepSize * (jacobOutput.pinv() * dx)
#Use change in q to approach the best joint configuration
newQ = Matrix(q) + dq
#Exit solution attempt if too many nudges are occurring relative to the maximum number of gradient descent steps
#We don't want to spend too much time on a solution that is boarderline not reachable
if numNudges >= self._subIterations / 2:
break
#Check the new configuration to make sure it is in bounds, and nudge it back in bounds if so
ind = 0
for angle in newQ:
nudged = True
if not self._rev_joints[ind]:
if angle < self._joint_limits[ind][0]:
q[ind] = self._joint_limits[ind][0]
elif angle > self._joint_limits[ind][1]:
q[ind] = self._joint_limits[ind][1]
else:
q[ind] = angle
nudged = False
else:
if angle < -1 * self._joint_limits[ind][1]:
q[ind] = -1 * self._joint_limits[ind][1]
elif angle > -1 * self._joint_limits[ind][0]:
q[ind] = -1 * self._joint_limits[ind][0]
else:
q[ind] = angle
nudged = False
ind += 1
if nudged:
numNudges += 1
#Recalculate the distance to the goal angle post nudging to check if it is the best solution yet
x = self.getEE(q)
x = x.col(-1)
x.row_del(-1)
dx = goal - x
#Store the configuration if it is the best one we've seen thusfar
if dx.norm() < bestDX.norm():
print("Best dx (" + str(bestDX.norm()) +
") replaced with new diff: " + str(dx.norm()))
bestDX = dx
bestQ = q
else:
print("dx not replaced")
return bestQ
def setAnglesAndGrip(self, qVector, grip):
"""Send a vector of joint angles and a gripper position over serial to the robot
Returns the new joint angles"""
#Clear the serial connection for a new exchange
self.ser.flushInput()
self.ser.flushOutput()
#Combine joint vector into a single string command for sending over serial
intQ = []
for floatVal in qVector:
intQ.append(int(round(floatVal)))
command = str(intQ[0]) + ',' + str(intQ[1]) + ',' + str(
intQ[2]) + ',' + str(intQ[3]) + ',' + str(
intQ[4]) + ',' + str(grip) + '\n'
print("Sending: " + command)
self.ser.write(command.encode('utf-8'))
#wait until a repsonse if found from the arduino
OK = 'no'
while (OK != 'd'):
print("WaitingOnResponse: " + OK)
OK = self.ser.read(1).decode("utf-8")
print("ReceivedResponse")
return intQ + [grip]
def convertIKQToDegrees(self, q):
"""convert a vector of joint angles in radians from IK solution to degrees suitable for sending to the servos"""
qNew = []
ind = 0
for angle in q:
if not self._rev_joints[ind]:
qNew.append((angle * (180.0 / np.pi)) + self._home[ind])
else:
qNew.append(self._home[ind] - (angle * (180.0 / np.pi)))
ind += 1
return qNew
def home(self):
"""Send robot to home position and record angles"""
self._current = self.setAnglesAndGrip(self._home, self._grip_neutral)
def moveToGoalAbsolute(self, point):
"""Send robot to the given goal position and record angles"""
pointAsVector = [point.x, point.y, point.z]
bestConfig = self.solveForEFTranslation(pointAsVector)
print("output angles")
print(bestConfig)
print("fk of output angles")
print(self.getEE(bestConfig))
qNew = self.convertIKQToDegrees(bestConfig)
self._current = self.setAnglesAndGrip(qNew, self._grip_neutral)
def moveToGoalRelative(self, point):
"""Adjust robot by the given differential vector and record angles"""
pointAsVector = [point.x, point.y, point.z]
x = self.getEE(self._current)
x = x.col(-1)
x.row_del(-1)
relativePoint = x + Matrix(pointAsVector)
bestConfig = self.solveForEFTranslation(relativePoint)
print("output angles")
print(bestConfig)
print("fk of output angles")
print(self.getEE(bestConfig))
qNew = self.convertIKQToDegrees(bestConfig)
self._current = self.setAnglesAndGrip(qNew, self._grip_neutral)
def test_lambdify_matrix():
x = Symbol("x")
f = lambdify(x, Matrix([[x, 2 * x], [1, 2]]), "numpy")
assert (f(1) == matrix([[1, 2], [1, 2]])).all()
def test_vector():
"""
Tests the effects of orientation of coordinate systems on
basic vector operations.
"""
N = CoordSysCartesian('N')
A = N.orient_new_axis('A', q1, N.k)
B = A.orient_new_axis('B', q2, A.i)
C = B.orient_new_axis('C', q3, B.j)
#Test to_matrix
v1 = a*N.i + b*N.j + c*N.k
assert v1.to_matrix(A) == Matrix([[ a*cos(q1) + b*sin(q1)],
[-a*sin(q1) + b*cos(q1)],
[ c]])
#Test dot
assert N.i.dot(A.i) == cos(q1)
assert N.i.dot(A.j) == -sin(q1)
assert N.i.dot(A.k) == 0
assert N.j.dot(A.i) == sin(q1)
assert N.j.dot(A.j) == cos(q1)
assert N.j.dot(A.k) == 0
assert N.k.dot(A.i) == 0
assert N.k.dot(A.j) == 0
assert N.k.dot(A.k) == 1
assert N.i.dot(A.i + A.j) == -sin(q1) + cos(q1) == \
(A.i + A.j).dot(N.i)
assert A.i.dot(C.i) == cos(q3)
assert A.i.dot(C.j) == 0
assert A.i.dot(C.k) == sin(q3)
assert A.j.dot(C.i) == sin(q2)*sin(q3)
assert A.j.dot(C.j) == cos(q2)
assert A.j.dot(C.k) == -sin(q2)*cos(q3)
assert A.k.dot(C.i) == -cos(q2)*sin(q3)
assert A.k.dot(C.j) == sin(q2)
assert A.k.dot(C.k) == cos(q2)*cos(q3)
#Test cross
assert N.i.cross(A.i) == sin(q1)*A.k
assert N.i.cross(A.j) == cos(q1)*A.k
assert N.i.cross(A.k) == -sin(q1)*A.i - cos(q1)*A.j
assert N.j.cross(A.i) == -cos(q1)*A.k
assert N.j.cross(A.j) == sin(q1)*A.k
assert N.j.cross(A.k) == cos(q1)*A.i - sin(q1)*A.j
assert N.k.cross(A.i) == A.j
assert N.k.cross(A.j) == -A.i
assert N.k.cross(A.k) == Vector.zero
assert N.i.cross(A.i) == sin(q1)*A.k
assert N.i.cross(A.j) == cos(q1)*A.k
assert N.i.cross(A.i + A.j) == sin(q1)*A.k + cos(q1)*A.k
assert (A.i + A.j).cross(N.i) == (-sin(q1) - cos(q1))*N.k
assert A.i.cross(C.i) == sin(q3)*C.j
assert A.i.cross(C.j) == -sin(q3)*C.i + cos(q3)*C.k
assert A.i.cross(C.k) == -cos(q3)*C.j
assert C.i.cross(A.i) == (-sin(q3)*cos(q2))*A.j + \
(-sin(q2)*sin(q3))*A.k
assert C.j.cross(A.i) == (sin(q2))*A.j + (-cos(q2))*A.k
assert express(C.k.cross(A.i), C).trigsimp() == cos(q3)*C.j
def solveForEFTranslation(self, goalX):
"""Get the joint configuration that will get the robot's end effector to (or close to) the goal position.
Takes in a 3d vector as a goal position
Returns a vector of joint angles """
iterations = 0
goal = Matrix(goalX)
dx = Matrix([1, 1, 1])
bestDX = Matrix([999, 999, 999])
bestQ = []
#Iterate until you find an acceptable end effector position or hit the max iterations
while iterations < self._maxIterations and bestDX.norm(
) > self._goalAccuracy:
iterations += 1
#Initialize to a random initial configuration (avoid local minimums)
q = [
self._joint_limits[0][0] +
random.random() * self._joint_limits[0][1],
self._joint_limits[1][0] +
random.random() * self._joint_limits[1][1],
self._joint_limits[2][0] +
random.random() * self._joint_limits[2][1],
self._joint_limits[3][0] +
random.random() * self._joint_limits[3][1],
self._joint_limits[4][0] +
random.random() * self._joint_limits[4][1]
]
k = 0
#Perfrom gradient descent over the next subIteration number of steps
numNudges = 0
while k < self._subIterations:
k += 1
#Get the difference of the end effector from the goal position
x = self.getEE(q)
x = x.col(-1)
x.row_del(-1)
dx = goal - x
#Use the inverse (pseduo, as this isn't square) of the jacobian at the current joint configuration to get a
#change in q that lowers the difference of the end effector from the goal position
substituteDict = {
self._q1: q[0],
self._q2: q[1],
self._q3: q[2],
self._q4: q[3],
self._q5: q[4]
}
jacobOutput = self.translationJacobian.evalf(
subs=substituteDict)
dq = self._stepSize * (jacobOutput.pinv() * dx)
#Use change in q to approach the best joint configuration
newQ = Matrix(q) + dq
#Exit solution attempt if too many nudges are occurring relative to the maximum number of gradient descent steps
#We don't want to spend too much time on a solution that is boarderline not reachable
if numNudges >= self._subIterations / 2:
break
#Check the new configuration to make sure it is in bounds, and nudge it back in bounds if so
ind = 0
for angle in newQ:
nudged = True
if not self._rev_joints[ind]:
if angle < self._joint_limits[ind][0]:
q[ind] = self._joint_limits[ind][0]
elif angle > self._joint_limits[ind][1]:
q[ind] = self._joint_limits[ind][1]
else:
q[ind] = angle
nudged = False
else:
if angle < -1 * self._joint_limits[ind][1]:
q[ind] = -1 * self._joint_limits[ind][1]
elif angle > -1 * self._joint_limits[ind][0]:
q[ind] = -1 * self._joint_limits[ind][0]
else:
q[ind] = angle
nudged = False
ind += 1
if nudged:
numNudges += 1
#Recalculate the distance to the goal angle post nudging to check if it is the best solution yet
x = self.getEE(q)
x = x.col(-1)
x.row_del(-1)
dx = goal - x
#Store the configuration if it is the best one we've seen thusfar
if dx.norm() < bestDX.norm():
print("Best dx (" + str(bestDX.norm()) +
") replaced with new diff: " + str(dx.norm()))
bestDX = dx
bestQ = q
else:
print("dx not replaced")
return bestQ
def curvature(Rmn):
return Rmn.ud(0, 0) + Rmn.ud(1, 1) + Rmn.ud(2, 2) + Rmn.ud(3, 3)
B = Function('B')
A = Function('A')
t = Symbol('t')
r = Symbol('r')
theta = Symbol('theta')
phi = Symbol('phi')
#general, spherically symmetric metric
gdd = Matrix(((-B(r), 0, 0, 0), (0, A(r), 0, 0), (0, 0, r**2, 0),
(0, 0, 0, r**2 * sin(theta)**2)))
g = MT(gdd)
X = (t, r, theta, phi)
Gamma = G(g, X)
Rmn = Ricci(Riemann(Gamma, X), X)
def pprint_Gamma_udd(i, k, l):
pprint(Eq(Symbol('Gamma^%i_%i%i' % (i, k, l)), Gamma.udd(i, k, l)))
def pprint_Rmn_dd(i, j):
pprint(Eq(Symbol('R_%i%i' % (i, j)), Rmn.dd(i, j)))
def test_as_immutable():
X = Matrix([[1,2], [3,4]])
assert X.as_immutable() == ImmutableMatrix([[1,2],[3,4]])
def test_issue_14950():
x = Matrix(symbols('t s'))
x0 = Matrix([17, 23])
eqn = x + x0
assert nsolve(eqn, x, x0) == -x0
assert nsolve(eqn.T, x.T, x0.T) == -x0
def test_Matrix2():
x = Symbol("x")
m = Matrix([[x, x**2], [5, 2 / x]])
assert (matrix(m.subs(x, 2)) == matrix([[2, 4], [5, 1]])).all()
m = Matrix([[sin(x), x**2], [5, 2 / x]])
assert (matrix(m.subs(x, 2)) == matrix([[sin(2), 4], [5, 1]])).all()
def diop_quadratic(var, coeff, t):
"""
Solves quadratic diophantine equations, i.e equations of the form
Ax**2 + Bxy + Cy**2 + Dx + Ey + F = 0. Returns a set containing
the tuples (x, y) which contains the solutions.
Usage
=====
diop_quadratic(var, coeff) -> var is a list of variables and
coeff is a dictionary containing coefficients of the symbols.
Details
=======
``var`` a list which contains two variables x and y.
``coeff`` a dict which generally contains six key value pairs.
The set of keys is {x**2, y**2, x*y, x, y, Integer(1)}.
``t`` the parameter to be used in the solution.
Examples
========
>>> from sympy.abc import x, y, t
>>> from sympy import Integer
>>> from sympy.solvers.diophantine import diop_quadratic
>>> diop_quadratic([x, y], {x**2: 1, y**2: 1, x*y: 0, x: 2, y: 2, Integer(1): 2}, t)
set([(-1, -1)])
References
==========
.. [1] http://www.alpertron.com.ar/METHODS.HTM
"""
x = var[0]
y = var[1]
for term in [x**2, y**2, x*y, x, y, Integer(1)]:
if term not in coeff.keys():
coeff[term] = Integer(0)
A = coeff[x**2]
B = coeff[x*y]
C = coeff[y**2]
D = coeff[x]
E = coeff[y]
F = coeff[Integer(1)]
d = igcd(A, igcd(B, igcd(C, igcd(D, igcd(E, F)))))
A = A // d
B = B // d
C = C // d
D = D // d
E = E // d
F = F // d
# (1) Linear case: A = B = C = 0 -> considered under linear diophantine equations
# (2) Simple-Hyperbolic case:A = C = 0, B != 0
# In this case equation can be converted to (Bx + E)(By + D) = DE - BF
# We consider two cases; DE - BF = 0 and DE - BF != 0
# More details, http://www.alpertron.com.ar/METHODS.HTM#SHyperb
l = set([])
if A == 0 and C == 0 and B != 0:
if D*E - B*F == 0:
if divisible(int(E), int(B)):
l.add((-E/B, t))
if divisible(int(D), int(B)):
l.add((t, -D/B))
else:
div = divisors(D*E - B*F)
div = div + [-term for term in div]
for d in div:
if divisible(int(d - E), int(B)):
x0 = (d - E) // B
if divisible(int(D*E - B*F), int(d)):
if divisible(int((D*E - B*F)// d - D), int(B)):
y0 = ((D*E - B*F) // d - D) // B
l.add((x0, y0))
# (3) Elliptical case: B**2 - 4AC < 0
# More Details, http://www.alpertron.com.ar/METHODS.HTM#Ellipse
# In this case x should lie between the roots of
# (B**2 - 4AC)x**2 + 2(BE - 2CD)x + (E**2 - 4CF) = 0
elif B**2 - 4*A*C < 0:
z = symbols("z", real=True)
roots = solve((B**2 - 4*A*C)*z**2 + 2*(B*E - 2*C*D)*z + E**2 - 4*C*F)
solve_y = lambda x, e: (-(B*x + E) + e*sqrt((B*x + E)**2 - 4*C*(A*x**2 + D*x + F)))/(2*C)
if len(roots) == 1 and isinstance(roots[0], Integer):
x_vals = [roots[0]]
elif len(roots) == 2:
x_vals = [i for i in range(ceiling(min(roots)), ceiling(max(roots)))] # ceiling = floor +/- 1
else:
x_vals = []
for x0 in x_vals:
if isinstance(sqrt((B*x0 + E)**2 - 4*C*(A*x0**2 + D*x0 + F)), Integer):
if isinstance(solve_y(x0, 1), Integer):
l.add((Integer(x0), solve_y(x0, 1)))
if isinstance(solve_y(x0, -1), Integer):
l.add((Integer(x0), solve_y(x0, -1)))
# (4) Parabolic case: B**2 - 4*A*C = 0
# There are two subcases to be considered in this case.
# sqrt(c)D - sqrt(a)E = 0 and sqrt(c)D - sqrt(a)E != 0
# More Details, http://www.alpertron.com.ar/METHODS.HTM#Parabol
elif B**2 - 4*A*C == 0:
g = igcd(A, C)
g = abs(g) * sign(A)
a = A // g
b = B // g
c = C // g
e = sign(B/A)
if e*sqrt(c)*D - sqrt(a)*E == 0:
z = symbols("z", real=True)
roots = solve(sqrt(a)*g*z**2 + D*z + sqrt(a)*F)
for root in roots:
if isinstance(root, Integer):
l.add((diop_solve(sqrt(a)*x + e*sqrt(c)*y - root)[x], diop_solve(sqrt(a)*x + e*sqrt(c)*y - root)[y]))
elif isinstance(e*sqrt(c)*D - sqrt(a)*E, Integer):
solve_x = lambda u: e*sqrt(c)*g*(sqrt(a)*E - e*sqrt(c)*D)*t**2 - (E + 2*e*sqrt(c)*g*u)*t\
- (e*sqrt(c)*g*u**2 + E*u + e*sqrt(c)*F) // (e*sqrt(c)*D - sqrt(a)*E)
solve_y = lambda u: sqrt(a)*g*(e*sqrt(c)*D - sqrt(a)*E)*t**2 + (D + 2*sqrt(a)*g*u)*t \
+ (sqrt(a)*g*u**2 + D*u + sqrt(a)*F) // (e*sqrt(c)*D - sqrt(a)*E)
for z0 in range(0, abs(e*sqrt(c)*D - sqrt(a)*E)):
if divisible(sqrt(a)*g*z0**2 + D*z0 + sqrt(a)*F, e*sqrt(c)*D - sqrt(a)*E):
l.add((solve_x(z0), solve_y(z0)))
# (5) B**2 - 4*A*C > 0
elif B**2 - 4*A*C > 0:
# Method used when B**2 - 4*A*C is a square, is descibed in p. 6 of the below paper
# by John P. Robertson.
# http://www.jpr2718.org/ax2p.pdf
if isinstance(sqrt(B**2 - 4*A*C), Integer):
if A != 0:
r = sqrt(B**2 - 4*A*C)
u, v = symbols("u, v", integer=True)
eq = simplify(4*A*r*u*v + 4*A*D*(B*v + r*u + r*v - B*u) + 2*A*4*A*E*(u - v) + 4*A*r*4*A*F)
sol = diop_solve(eq, t)
sol = list(sol)
for solution in sol:
s0 = solution[0]
t0 = solution[1]
x_0 = S(B*t0 + r*s0 + r*t0 - B*s0)/(4*A*r)
y_0 = S(s0 - t0)/(2*r)
if isinstance(s0, Symbol) or isinstance(t0, Symbol):
if check_param(x_0, y_0, 4*A*r, t) != (None, None):
l.add((check_param(x_0, y_0, 4*A*r, t)[0], check_param(x_0, y_0, 4*A*r, t)[1]))
elif divisible(B*t0 + r*s0 + r*t0 - B*s0, 4*A*r):
if divisible(s0 - t0, 2*r):
if is_solution_quad(var, coeff, x_0, y_0):
l.add((x_0, y_0))
else:
var[0], var[1] = var[1], var[0] # Interchange x and y
s = diop_quadratic(var, coeff, t)
while len(s) > 0:
sol = s.pop()
l.add((sol[1], sol[0]))
else:
# In this case equation can be transformed into a Pell equation
A, B = _transformation_to_pell(var, coeff)
D, N = _find_DN(var, coeff)
solns_pell = diop_pell(D, N)
n = symbols("n", integer=True)
a = diop_pell(D, 1)
T = a[0][0]
U = a[0][1]
if (isinstance(A[0], Integer) and isinstance(A[1], Integer) and isinstance(A[2], Integer)
and isinstance(A[3], Integer) and isinstance(B[0], Integer) and isinstance(B[1], Integer)):
for sol in solns_pell:
r = sol[0]
s = sol[1]
x_n = S((r + s*sqrt(D))*(T + U*sqrt(D))**n + (r - s*sqrt(D))*(T - U*sqrt(D))**n)/2
y_n = S((r + s*sqrt(D))*(T + U*sqrt(D))**n - (r - s*sqrt(D))*(T - U*sqrt(D))**n)/(2*sqrt(D))
x_n, y_n = (A*Matrix([x_n, y_n]) + B)[0], (A*Matrix([x_n, y_n]) + B)[1]
l.add((x_n, y_n))
else:
L = ilcm(S(A[0]).q, ilcm(S(A[1]).q, ilcm(S(A[2]).q, ilcm(S(A[3]).q, ilcm(S(B[0]).q, S(B[1]).q)))))
k = 0
done = False
T_k = T
U_k = U
while not done:
k = k + 1
if (T_k - 1) % L == 0 and U_k % L == 0:
done = True
T_k, U_k = T_k*T + D*U_k*U, T_k*U + U_k*T
for soln in solns_pell:
x_0 = soln[0]
y_0 = soln[1]
x_i = x_0
y_i = y_0
for i in range(k):
X = (A*Matrix([x_i, y_i]) + B)[0]
Y = (A*Matrix([x_i, y_i]) + B)[1]
if isinstance(X, Integer) and isinstance(Y, Integer):
if is_solution_quad(var, coeff, X, Y):
x_n = S( (x_i + sqrt(D)*y_i)*(T + sqrt(D)*U)**(n*L) + (x_i - sqrt(D)*y_i)*(T - sqrt(D)*U)**(n*L) )/ 2
y_n = S( (x_i + sqrt(D)*y_i)*(T + sqrt(D)*U)**(n*L) - (x_i - sqrt(D)*y_i)*(T - sqrt(D)*U)**(n*L) )/ (2*sqrt(D))
x_n, y_n = (A*Matrix([x_n, y_n]) + B)[0], (A*Matrix([x_n, y_n]) + B)[1]
l.add((x_n, y_n))
x_i = x_i*T + D*U*y_i
y_i = x_i*U + y_i*T
return l
import numpy as np
from sympy import simplify, Matrix, var
xi = var('xi')
eta = var('eta')
print('Chebyshev Polynomials of the Second Kind')
u = [1, 2 * xi]
for i in range(2, 15):
ui = simplify(2 * xi * u[i - 1] - u[i - 2])
u.append(ui)
for i, ui in enumerate(u):
print('u({0}) = {1}'.format(i, ui))
with open('cheb.txt', 'w') as f:
f.write('Number of terms: {0}\n\n'.format(len(u)))
f.write(','.join(map(str, u)).replace('**', '^') + '\n\n')
f.write(','.join(map(str, u)).replace('xi', 'eta').replace('**', '^'))
print np.outer(u, [ui.subs({xi: eta}) for ui in u])
if False:
m = Matrix([[2 * xi, 1, 0, 0], [1, 2 * xi, 1, 0], [0, 1, 2 * xi, 1],
[0, 0, 1, 2 * xi]])
print('m.det() {0}'.format(simplify(m.det())))
def _transformation_to_pell(var, coeff):
x = var[0]
y = var[1]
a = coeff[x**2]
b = coeff[x*y]
c = coeff[y**2]
d = coeff[x]
e = coeff[y]
f = coeff[Integer(1)]
g = igcd(a, igcd(b, igcd(c, igcd(d, igcd(e, f)))))
a = a // g
b = b // g
c = c // g
d = d // g
e = e // g
f = f // g
X, Y = symbols("X, Y", integer=True)
if b != Integer(0):
B = (S(2*a)/b).p
C = (S(2*a)/b).q
A = (S(a)/B**2).p
T = (S(a)/B**2).q
# eq_1 = A*B*X**2 + B*(c*T - A*C**2)*Y**2 + d*T*X + (B*e*T - d*T*C)*Y + f*T*B
coeff = {X**2: A*B, X*Y: 0, Y**2: B*(c*T - A*C**2), X: d*T, Y: B*e*T - d*T*C, Integer(1): f*T*B}
A_0, B_0 = _transformation_to_pell([X, Y], coeff)
return Matrix(2, 2, [S(1)/B, -S(C)/B, 0, 1])*A_0, Matrix(2, 2, [S(1)/B, -S(C)/B, 0, 1])*B_0
else:
if d != Integer(0):
B = (S(2*a)/d).p
C = (S(2*a)/d).q
A = (S(a)/B**2).p
T = (S(a)/B**2).q
# eq_2 = A*X**2 + c*T*Y**2 + e*T*Y + f*T - A*C**2
coeff = {X**2: A, X*Y: 0, Y**2: c*T, X: 0, Y: e*T, Integer(1): f*T - A*C**2}
A_0, B_0 = _transformation_to_pell([X, Y], coeff)
return Matrix(2, 2, [S(1)/B, 0, 0, 1])*A_0, Matrix(2, 2, [S(1)/B, 0, 0, 1])*B_0 + Matrix([-S(C)/B, 0])
else:
if e != Integer(0):
B = (S(2*c)/e).p
C = (S(2*c)/e).q
A = (S(c)/B**2).p
T = (S(c)/B**2).q
# eq_3 = a*T*X**2 + A*Y**2 + f*T - A*C**2
coeff = {X**2: a*T, X*Y: 0, Y**2: A, X: 0, Y: 0, Integer(1): f*T - A*C**2}
A_0, B_0 = _transformation_to_pell([X, Y], coeff)
return Matrix(2, 2, [1, 0, 0, S(1)/B])*A_0, Matrix(2, 2, [1, 0, 0, S(1)/B])*B_0 + Matrix([0, -S(C)/B])
else:
# TODO: pre-simplification: Not necessary but may simplify
# the equation.
return Matrix(2, 2, [S(1)/a, 0, 0, 1]), Matrix([0, 0])
dt = 1.0/50.0 # Sample Rate of the Measurements is 50Hz
dtGPS=1.0/10.0 # Sample Rate of GPS is 10Hz
# ### Developing the math behind dynamic model
#
# All symbolic calculations are made with [Sympy](http://nbviewer.ipython.org/github/jrjohansson/scientific-python-lectures/blob/master/Lecture-5-Sympy.ipynb). Thanks!
# In[112]:
vs, psis, dpsis, dts, xs, ys, lats, lons = symbols('v \psi \dot\psi T x y lat lon')
gs = Matrix([[xs+(vs/dpsis)*(sin(psis+dpsis*dts)-sin(psis))],
[ys+(vs/dpsis)*(-cos(psis+dpsis*dts)+cos(psis))],
[psis+dpsis*dts],
[vs],
[dpsis]])
state = Matrix([xs,ys,psis,vs,dpsis])
# ## Dynamic Function $g$
#
# This formulas calculate how the state is evolving from one to the next time step
# In[113]:
gs
# ### Calculate the Jacobian of the Dynamic function $g$ with respect to the state vector $x$