def propagate(ham, psi, delta_t, tridiag=False):
"""Propagage the wave function `psi` by time `delta_t` using the hamiltonian
`ham`. Uses the Crank-Nicolson method.
"""
if sp.isspmatrix_dia(ham):
# Optimization for banded matrices
denom = ham.copy()
denom.data *= 0.5j * delta_t
denom.data[denom.offsets == 0, :] += 1.0
numer = sp.eye(ham.shape[0],
format='csr') - 0.5j * ham.tocsr() * delta_t
denom, l, u = _make_banded_matrix(denom.todia())
temp = la.solve_banded((l, u), denom, psi)
return numer.dot(temp)
if sp.isspmatrix(ham):
denom = sp.eye(ham.shape[0]) + 0.5j * ham * delta_t
numer = sp.eye(ham.shape[0]) - 0.5j * ham * delta_t
temp = sla.spsolve(denom, psi)
return numer.dot(temp)
else:
denom = np.eye(ham.shape[0]) + 0.5j * ham * delta_t
numer = np.eye(ham.shape[0]) - 0.5j * ham * delta_t
temp = la.solve(denom, psi)
return np.dot(numer, temp)
def test_csr(self):
x = sparse.csr_matrix(
(cupy.array([], 'f'),
cupy.array([], 'i'),
cupy.array([0], 'i')),
shape=(0, 0), dtype='f')
self.assertFalse(sparse.isspmatrix_dia(x))
def inv(self, x):
if self.sparse:
if sp.isspmatrix_dia(x) and x.offsets == [0]:
return sp.diags(1 / (x.diagonal()))
else:
return sparselinalg.inv(x)
else:
return np.linalg.inv(x)
def test_loading_and_storing_empty_containers(self):
filename = make_temp_dir('empty_containers.hdf5')
traj = Trajectory(filename=filename, add_time=True)
# traj.f_add_parameter('empty.dict', {})
# traj.f_add_parameter('empty.list', [])
traj.f_add_parameter(ArrayParameter, 'empty.tuple', ())
traj.f_add_parameter(ArrayParameter, 'empty.array',
np.array([], dtype=float))
spsparse_csc = spsp.csc_matrix((2, 10))
spsparse_csr = spsp.csr_matrix((6660, 660))
spsparse_bsr = spsp.bsr_matrix((3330, 2220))
spsparse_dia = spsp.dia_matrix((1230, 1230))
traj.f_add_parameter(SparseParameter, 'empty.csc', spsparse_csc)
traj.f_add_parameter(SparseParameter, 'empty.csr', spsparse_csr)
traj.f_add_parameter(SparseParameter, 'empty.bsr', spsparse_bsr)
traj.f_add_parameter(SparseParameter, 'empty.dia', spsparse_dia)
traj.f_add_result(SparseResult,
'empty.all',
dict={},
list=[],
series=pd.Series(),
frame=pd.DataFrame(),
panel=pd.Panel(),
**traj.par.f_to_dict(short_names=True,
fast_access=True))
traj.f_store()
newtraj = load_trajectory(index=-1, filename=filename)
newtraj.f_load(load_data=2)
epg = newtraj.par.empty
self.assertTrue(type(epg.tuple) is tuple)
self.assertTrue(len(epg.tuple) == 0)
self.assertTrue(type(epg.array) is np.ndarray)
self.assertTrue(epg.array.size == 0)
self.assertTrue(spsp.isspmatrix_csr(epg.csr))
self.assertTrue(epg.csr.size == 0)
self.assertTrue(spsp.isspmatrix_csc(epg.csc))
self.assertTrue(epg.csc.size == 0)
self.assertTrue(spsp.isspmatrix_bsr(epg.bsr))
self.assertTrue(epg.bsr.size == 0)
self.assertTrue(spsp.isspmatrix_dia(epg.dia))
self.assertTrue(epg.dia.size == 0)
self.compare_trajectories(traj, newtraj)
def sparse_matrix_report(m):
print(repr(m))
print('Number of non-zeros :', m.nnz)
print('Sparsity :', 1 - m.nnz / (m.shape[0] * m.shape[1]))
if isspmatrix_csr(m) or isspmatrix_csc(m):
print('data length : {} ({})'.format(len(m.data),
m.data.dtype))
print('indptr length : {} ({})'.format(len(m.indptr),
m.indptr.dtype))
print('indices length : {} ({})'.format(len(m.indices),
m.indices.dtype))
print('Size :',
size(m.data.nbytes + m.indptr.nbytes + m.indices.nbytes))
print('10 x 10 preview:')
print(m[:10, :10].toarray())
elif isspmatrix_bsr(m):
print('data length : {} ({})'.format(len(m.data),
m.data.dtype))
print('indptr length : {} ({})'.format(len(m.indptr),
m.indptr.dtype))
print('indices length : {} ({})'.format(len(m.indices),
m.indices.dtype))
print('blocksize length : {}'.format(m.blocksize))
print('Size :',
size(m.data.nbytes + m.indptr.nbytes + m.indices.nbytes))
print('preview:')
print(m)
elif isspmatrix_coo(m):
print('data length : {} ({})'.format(len(m.data),
m.data.dtype))
print('row length : {} ({})'.format(len(m.row), m.row.dtype))
print('col length : {} ({})'.format(len(m.col), m.col.dtype))
print('Size :',
size(m.data.nbytes + m.row.nbytes + m.col.nbytes))
print('preview:')
print(m)
elif isspmatrix_dok(m):
print('Size :', size(sys.getsizeof(m)))
print('10 x 10 preview:')
print(m[:10, :10].toarray())
elif isspmatrix_dia(m):
print('data length : {} ({})'.format(len(m.data),
m.data.dtype))
print('Offsets : {} ({})'.format(len(m.offsets),
m.offsets.dtype))
print('Size :', size(m.data.nbytes + m.offsets.nbytes))
print('(no preview)')
elif isspmatrix_lil(m):
print('data length : {} ({})'.format(len(m.data),
m.data.dtype))
print('rows : {} ({})'.format(len(m.rows),
m.rows.dtype))
print('Size :', size(m.data.nbytes + m.rows.nbytes))
print('(no preview)')
def logdet(self, x):
if sp.issparse(x):
if sp.isspmatrix_dia(x) and x.offsets == [0]:
res = sum(np.log(x.diagonal()))
else:
print('Not implemented')
res = sum(np.log(x.diagonal()))
# factor = cholesky(x)
# res = factor.logdet()
return res
else:
return np.linalg.slogdet(x)[1]
def add_varinvs(self, dists):
for name in dists:
if name[0] == 'q':
if 'var' in dists[name]:
if sp.issparse(dists[name]['var']):
if sp.isspmatrix_dia(dists[name]['var']):
return sp.dia_matrix(1 /
dists[name]['var'].diagonal())
else:
return add_varinv(dists[name], sparselinalg.inv)
else:
add_varinv(dists[name], np.linalg.inv)
def zero_col(A, col):
"""Sets the specified column of A to zero"""
if sparse.issparse(A):
if sparse.isspmatrix_coo(A) or sparse.isspmatrix_dia(A):
A = A.tolil()
#doesn't support slicing
for i in xrange(A.shape[0]):
A[i, col] = 0
return A
else:
A[:, col] = np.zeros(A.shape[0])
return A
def zero_col(A,col): """Sets the specified column of A to zero""" if sparse.issparse(A): if sparse.isspmatrix_coo(A) or sparse.isspmatrix_dia(A): A = A.tolil() #doesn't support slicing for i in xrange(A.shape[0]): A[i,col] = 0 return A else: A[:,col] = np.zeros(A.shape[0]) return A
def test_loading_and_storing_empty_containers(self):
filename = make_temp_dir('empty_containers.hdf5')
traj = Trajectory(filename=filename)
# traj.f_add_parameter('empty.dict', {})
# traj.f_add_parameter('empty.list', [])
traj.f_add_parameter(ArrayParameter, 'empty.tuple', ())
traj.f_add_parameter(ArrayParameter, 'empty.array', np.array([], dtype=float))
spsparse_csc = spsp.csc_matrix((2,10))
spsparse_csr = spsp.csr_matrix((6660,660))
spsparse_bsr = spsp.bsr_matrix((3330,2220))
spsparse_dia = spsp.dia_matrix((1230,1230))
traj.f_add_parameter(SparseParameter, 'empty.csc', spsparse_csc)
traj.f_add_parameter(SparseParameter, 'empty.csr', spsparse_csr)
traj.f_add_parameter(SparseParameter, 'empty.bsr', spsparse_bsr)
traj.f_add_parameter(SparseParameter, 'empty.dia', spsparse_dia)
traj.f_add_result(SparseResult, 'empty.all', dict={}, list=[],
series = pd.Series(),
frame = pd.DataFrame(),
panel = pd.Panel(),
**traj.par.f_to_dict(short_names=True, fast_access=True))
traj.f_store()
newtraj = load_trajectory(index=-1, filename=filename)
newtraj.f_load(load_data=2)
epg = newtraj.par.empty
self.assertTrue(type(epg.tuple) is tuple)
self.assertTrue(len(epg.tuple) == 0)
self.assertTrue(type(epg.array) is np.ndarray)
self.assertTrue(epg.array.size == 0)
self.assertTrue(spsp.isspmatrix_csr(epg.csr))
self.assertTrue(epg.csr.size == 0)
self.assertTrue(spsp.isspmatrix_csc(epg.csc))
self.assertTrue(epg.csc.size == 0)
self.assertTrue(spsp.isspmatrix_bsr(epg.bsr))
self.assertTrue(epg.bsr.size == 0)
self.assertTrue(spsp.isspmatrix_dia(epg.dia))
self.assertTrue(epg.dia.size == 0)
self.compare_trajectories(traj, newtraj)
def test_dia(self):
x = sparse.dia_matrix((cupy.array([], 'f'), cupy.array([0], 'i')),
shape=(0, 0),
dtype='f')
assert sparse.isspmatrix_dia(x) is True
def solve(self, q,dq,dt): """Takes sensed q,dq, timestep dt and returns qdes and dqdes in joint space. """ for task in self.taskList: task.updateState(q,dq,dt) # priority 1 if not hasattr(self,'timingStats'): self.timingStats = defaultdict(int) self.timingStats['count'] += 1 t1 = time.time() J1 = self.getStackedJacobian(q,dq,1) v1 = self.getStackedVelocity(q,dq,dt,1) (A,b) = self.getMotionModel(q,dq,dt) if self.activeDofs != None: A = select_cols(A,self.activeDofs) if sparse.isspmatrix_coo(A) or sparse.isspmatrix_dia(A): A = A.tocsr() t2 = time.time() self.timingStats['get jac/vel p1'] += t2-t1 J2 = self.getStackedJacobian(q,dq,2) if J2 is not None: V2 = self.getStackedVelocity(q,dq,dt,2) t3 = time.time() self.timingStats['get jac/vel p2'] += t3-t2 #compute velocity limits vmax = self.robot.getVelocityLimits() vmin = vectorops.mul(vmax,-1.0) amax = self.robot.getAccelerationLimits() vref = dq if self.ulast == None else self.ulast for i,(v,vm,am) in enumerate(zip(vref,vmin,amax)): if v-dt*am > vm: vmin[i] = v-dt*am elif v < vm: #accelerate! vmin[i] = min(vm,v+dt*am) for i,(v,vm,am) in enumerate(zip(vref,vmax,amax)): if v-dt*am < vm: vmax[i] = v+dt*am elif v > vm: #decelerate! vmax[i] = max(vm,v-dt*am) for i,(l,u) in enumerate(zip(vmin,vmax)): assert l <= u if l > 0 or u < 0: print "Moving link:",self.robot.getLink(i).getName(),"speed",vref[i] #print zip(vmin,vmax) Aumin = np.array(vmin) - b Aumax = np.array(vmax) - b #print zip(Aumin.tolist(),Aumax.tolist()) J1A = J1.dot(A) J1b = J1.dot(b) if J2 == None: #just solve constrained least squares #J1*(A*u+b) = v1 #vmin < A*u + b < vmax u1 = constrained_lsqr(J1A,v1-J1b,A,Aumin,Aumax)[0] u2 = [0.0]*len(u1) t4 = time.time() self.timingStats['pinv jac p1'] += t4-t3 else: #solve equality constrained least squares #dq = A*u + b #J1*dq = v1 #J1*A*u + J1*b = v1 #least squares solve for u1: #J1*A*u1 = v1 - J1*b #vmin < A*u1 + b < vmax #need u to satisfy #Aact*u = bact #we know that u1 satisfies Aact*u = bact #let u = u1+u2 #=> u2 = (I - Aact^+ Aact) z = N*z #least squares solve for z: #J2*A*(u1+u2+b) = v2 #J2*A*N z = v2 - J2*(A*u1+b) (u1, active, activeRhs) = constrained_lsqr(J1A,v1-J1b,A,Aumin,Aumax) Aact = sparse.vstack([J1A]+[A[crow,:] for crow in active]).todense() #bact = np.hstack((v1-J1b,activeRhs)) J1Ainv = np.linalg.pinv(Aact) dq1 = A.dot(u1)+b if len(active)>0: print "Priority 1 active constraints:" for a in active: print self.robot.getLink(a).getName(),vmin[a],dq1[a],vmax[a] r1 = J1.dot(dq1)-v1 print "Op space controller solve" print " Residual 1",np.linalg.norm(r1) # priority 2 N = np.eye(len(dq)) - np.dot(J1Ainv, Aact) t4 = time.time() self.timingStats['pinv jac p1'] += t4-t3 u2 = [0.0]*len(u1) #print " Initial priority 2 task error",np.linalg.norm(V2-J2.dot(dq1)) J2A = J2.dot(A) J2AN = J2A.dot(N) AN = sparse.csr_matrix(np.dot(A.todense(),N)) #Note: N destroys sparsity V2_m_resid = np.ravel(V2 - J2.dot(dq1)) (z,active,activeRhs) = constrained_lsqr(J2AN,V2_m_resid,AN,vmin-dq1,vmax-dq1) t5 = time.time() self.timingStats['ls jac p2'] += t5-t4 u2 = np.ravel(np.dot(N, z)) #debug, should be close to zero #print " Nullspace projection error:",np.linalg.norm(J1A.dot(u2)) #this is the error in the nullspace of the first priority tasks dq2 = A.dot(u2) + dq1 #debug, should be equal to residual 2 printout above print " Residual 2",np.linalg.norm(J2.dot(dq2)-V2) #debug should be close to zero #print " Residual 2 in priority 1 frame",np.linalg.norm(J1.dot(dq2)-v1) if len(active)>0: print "Priority 2 active constraints:" for a in active: print self.robot.getLink(a).getName(),vmin[a],dq2[a],vmax[a] #compose the velocities together u = np.ravel((u1 + u2)) dqpred = A.dot(u)+b print " Residual 1 final",np.linalg.norm(np.ravel(J1.dot(dqpred))-v1) if J2 != None: print " Residual 2 final",np.linalg.norm(np.ravel(J2.dot(dqpred))-V2) u = u.tolist() #if self.activeDofs != None: # print "dqdes:",[self.dqdes[v] for v in self.activeDofs] self.qdes = vectorops.madd(q, u, dt) self.ulast = u t6 = time.time() self.timingStats['total']+=t6-t1 if self.timingStats['count']%10==0: n=self.timingStats['count'] print "OpSpace times (ms): vel/jac 1 %.2f inv 1 %.2f vel/jac 2 %.2f inv 2 %.2f total %.2f"%(self.timingStats['get jac/vel p1']/n*1000,self.timingStats['pinv jac p1']/n*1000,self.timingStats['get jac/vel p2']/n*1000,self.timingStats['ls jac p2']/n*1000,self.timingStats['total']/n*1000) return (self.qdes,u)
def constrained_lsqr(A,b,C,p,q): """Solves the least-squares problem min_x ||Ax-b||^2 with the constraints p<=Cx<=q. If there are more than one solution, picks the one with the lowest L-2 norm. The result is (x,activeSet,activeRhs) where activeSet lists the indices of the bounds that are met exactly and activeRhs lists the values. """ (x,istop,itn,normr,normar,norma,conda,normx) = sparse.linalg.lsmr(A,b,damp=1e-5) Cx = spdot(C,x) pmax,ipmax = max((pi-xi,i) for i,(pi,xi) in enumerate(zip(p,Cx))) qmax,iqmax = max((xi-qi,i) for i,(qi,xi) in enumerate(zip(q,Cx))) candidates = set(range(A.shape[1])) activeSet = [] activeRhs = [] AtA = None while pmax > 0 or qmax > 0: #add the most violated constraint to the active set #and re-solve cval = q[iqmax] crow = iqmax if pmax > qmax: cval = p[ipmax] crow = ipmax #print "Bound %d violation %f <= %f <= %f, active set size %d"%(crow,p[crow],Cx[crow],q[crow],len(activeSet)) candidates.remove(crow) activeSet.append(crow) activeRhs.append(cval) #form active set matrices if sparse.isspmatrix_coo(C) or sparse.isspmatrix_dia(C): C = C.tocsr() Atemp = sparse.vstack([A]+[C[crow,:] for crow in activeSet]) btemp = np.hstack((b,activeRhs)) #solve for A'x ~= b' with starting point x0 that satisfies Ax=b #for old active set #Let x=y+x0 #Solve for y that solves A'y ~= b'-A'x0 Atempx = Atemp.dot(x) (y,istop,itn,normr,normar,norma,conda,normx) = sparse.linalg.lsmr(Atemp,btemp-Atempx) if normr > 1e-4: #solve (AtA)x+C^T z = At b, Cx=d if AtA == None: AtA = A.T.dot(A) Ca = sparse.vstack([C[crow,:] for crow in activeSet]) kktMatrix = sparse.vstack(sparse.hstack((AtA,Ca.T)),sparse.hstack((Ca,sparse.csr_matrix((Ca.shape[0],Ca.shape[0]))))) (xz,istop,itn,normr,normar,norma,conda,normx) = sparse.linalg.lsmr(kktMatrix,np.hstack((np.dot(A.T,b),activeRhs))) if normr > 1e-4: print "Warning, could not solve for constraints exactly, error",normr x = xz[:x.shape[0]] else: x = x+y #(x,istop,itn,normr,normar,norma,conda,normx) = sparse.linalg.lsmr(Atemp,btemp) Cx = spdot(C,x) pmax,ipmax = max((p[i]-Cx[i],i) for i in candidates) qmax,iqmax = max((Cx[i]-q[i],i) for i in candidates) chtol = 1e-7 for i in xrange(len(x)): if Cx[i] < p[i]-chtol or Cx[i] > q[i]+chtol: print "Warning, constraint violation %d: %f <= %f <= %f"%(i,p[i],Cx[i],q[i]) return (x,activeSet,activeRhs)
def solve(self, A, d, **kwargs):
"""Solves linear systems with a tridiagonal coefficient matrix.
A solver that uses the Thomas algorithm (the Tridiagonal matrix
algorithm) for systems on the form
0
[b c ] [x] [d]
A.x = [a b c ] [x] = [d] = d.
[ a b c] [x] [d]
[ a b] [x] [d]
0
Parameters
----------
A : A sparse diagonal matrix (dia format) with shape n-by-p. The
coefficient matrix.
b : Numpy array, n-by-1. The right-hand-side vector.
Examples
--------
>>> import numpy as np
>>> import scipy.sparse as sparse
>>> import parsimony.utils.linalgs as linalgs
>>> np.random.seed(42)
>>>
>>> n = 10
>>> a = np.random.rand(n); a[-1] = 0.0
>>> b = np.random.rand(n)
>>> c = np.random.rand(n); c[0] = 0.0
>>> abc = np.vstack((a, b, c))
>>> A = sparse.dia_matrix((abc, [-1, 0, 1]), shape=(n, n))
>>> d = np.random.rand(n, 1)
>>>
>>> solver = linalgs.TridiagonalSolver()
>>> x = solver.solve(A, d)
>>> print(x)
[[ -1.84339326]
[ 4.62737333]
[-12.41571989]
[ 16.38029815]
[ 14.38143172]
[-14.58969243]
[ 6.21233944]
[ 1.34271395]
[ -1.63358708]
[ 4.88318651]]
>>> x_ = np.linalg.solve(A.toarray(), d)
>>> print(x_)
[[ -1.84339326]
[ 4.62737333]
[-12.41571989]
[ 16.38029815]
[ 14.38143172]
[-14.58969243]
[ 6.21233944]
[ 1.34271395]
[ -1.63358708]
[ 4.88318651]]
>>> np.linalg.norm(x - x_) < 5e-14
True
>>>
>>> import time
>>> n = 100
>>> a = np.random.rand(n); a[-1] = 0.0
>>> b = np.random.rand(n)
>>> c = np.random.rand(n); c[0] = 0.0
>>> abc = np.vstack((a, b, c))
>>> A = sparse.dia_matrix((abc, [-1, 0, 1]), shape=(n, n))
>>> d = np.random.rand(n, 1)
>>>
>>> t = time.time()
>>> x = solver.solve(A, d)
>>> print "Time:", time.time() - t # doctest: +SKIP
>>>
>>> t = time.time()
>>> x_ = np.linalg.solve(A.toarray(), d)
>>> print "Time:", time.time() - t # doctest: +SKIP
>>>
>>> np.linalg.norm(x - x_) < 5e-12
True
>>>
>>> n = 1000
>>> a = np.random.rand(n); a[-1] = 0.0
>>> b = np.random.rand(n)
>>> c = np.random.rand(n); c[0] = 0.0
>>> abc = np.vstack((a, b, c))
>>> A = sparse.dia_matrix((abc, [-1, 0, 1]), shape=(n, n))
>>> d = np.random.rand(n, 1)
>>>
>>> t = time.time()
>>> x = solver.solve(A, d)
>>> print "Time:", time.time() - t # doctest: +SKIP
>>>
>>> t = time.time()
>>> x_ = np.linalg.solve(A.toarray(), d)
>>> print "Time:", time.time() - t # doctest: +SKIP
>>>
>>> np.linalg.norm(x - x_) < 5e-9
True
"""
# TODO: Put in compiled code for speed.
if not sparse.isspmatrix_dia(A):
A = A.todia()
abc = A.data
a = abc[0, :]
b = abc[1, :]
c = abc[2, :]
if abc.dtype != np.float:
a = np.asarray(a, np.float)
b = np.asarray(b, np.float)
c = np.asarray(c, np.float)
n = len(a)
x = np.zeros(n)
# Decomposition and forward substitution.
c_ = np.zeros(n)
d_ = np.zeros(n)
i = 0
if abs(b[i]) < consts.TOLERANCE:
# TODO: Do this instead: In this case x0 is found trivially and we
# recurse to a problem of order n-1.
solver = SparseSolver()
return solver.solve(A, d)
c_[i + 1] = c[i + 1] / b[i]
d_[i] = d[i] / b[i]
for i in range(1, n - 1):
i_1 = i - 1
den = (b[i] - a[i_1] * c_[i])
if abs(den) < consts.TOLERANCE: # We cannot handle this case!
# TODO: Use algorithm for banded matrices instead!
solver = SparseSolver()
return solver.solve(A, d)
c_[i + 1] = c[i + 1] / den
d_[i] = (d[i] - a[i_1] * d_[i_1]) / den
i = n - 1
d_[i] = (d[i] - a[i - 1] * d_[i - 1]) / (b[i] - a[i - 1] * c_[i])
# Back substitution.
i = n - 1
x[i] = d_[i]
for i in reversed(range(n - 1)):
x[i] = d_[i] - c_[i + 1] * x[i + 1]
return x.reshape((n, 1))
def add(self, other, in_place=True, write_to_self=False):
"""
Add a matrix. The sum of self._raw_matrix with the passed StateMatrix (other).
Args:
other: another StateMatrix object of the same type as this object
in_place: If True, matrix addition is applied (in-place) to (self)
If False, a new copy will be returned.
Returns:
The sum of self with the passed StateMatrix (other).
"""
if write_to_self:
# update the reference matrix inside this object.
if not in_place:
result_mat = self.copy()
else:
result_mat = self
if isinstance(other, (StateMatrixNumpy, self.__class__)):
source_matrix = other
source_matrix_ref = other._raw_matrix
elif isinstance(other, np.ndarray):
source_matrix = other
source_matrix_ref = other
else:
raise TypeError(
"matrix has to be either 'StateMatrixNumpy', or 'StateMatrixSpSciPy', or 'np.ndarray' "
)
else:
# the target is the input matrix or a copy of it
if not in_place:
result_mat = other.copy()
else:
result_mat = other
source_matrix = self
source_matrix_ref = self._raw_matrix
#
# Check the result matrix format
if isinstance(result_mat, self.__class__):
result_mat_ref = result_mat.get_raw_matrix_ref()
#
# frmt = result_mat._raw_matrix.getformat()
# print('\n xxxxxxxxxxxxxxxxx \n %s \n xxxxxxxxxxxxxxxxx \n' % frmt)
#
if sparse.isspmatrix_bsr(result_mat._raw_matrix):
result_mat_ref = sparse.bsr_matrix(result_mat_ref +
source_matrix_ref)
elif sparse.isspmatrix_coo(result_mat._raw_matrix):
result_mat_ref = sparse.coo_matrix(result_mat_ref +
source_matrix_ref)
elif sparse.isspmatrix_csc(result_mat._raw_matrix):
result_mat_ref = sparse.csc_matrix(result_mat_ref +
source_matrix_ref)
elif sparse.isspmatrix_csr(result_mat._raw_matrix):
result_mat_ref = sparse.csr_matrix(result_mat_ref +
source_matrix_ref)
# print(result_mat._raw_matrix)
# print("is sparse: ", sparse.issparse(result_mat._raw_matrix))
elif sparse.isspmatrix_dia(result_mat._raw_matrix):
result_mat_ref = sparse.dia_matrix(result_mat_ref +
source_matrix_ref)
elif sparse.isspmatrix_dok(result_mat._raw_matrix):
result_mat_ref = sparse.dok_matrix(result_mat_ref +
source_matrix_ref)
elif sparse.isspmatrix_lil(result_mat._raw_matrix):
result_mat_ref = sparse.lil_matrix(result_mat_ref +
source_matrix_ref)
else:
raise TypeError(
"Unsupported Format! My format has been tapered with!")
result_mat.set_raw_matrix_ref(result_mat_ref)
result_mat._update_attributes()
elif isinstance(result_mat, StateMatrixNumpy):
result_mat_ref = result_mat.get_raw_matrix_ref()
if isinstance(source_matrix, self.__class__):
result_mat_ref = result_mat_ref + source_matrix_ref
try:
result_mat_ref = result_mat_ref.toarray()
except AttributeError:
result_mat_ref = np.asarray(result_mat_ref)
elif isinstance(source_matrix, (np.ndarray, StateMatrixNumpy)):
result_mat_ref = result_mat_ref + source_matrix_ref
result_mat.set_raw_matrix_ref(result_mat_ref)
elif isinstance(result_mat, np.ndarray):
result_mat_ref = result_mat
if isinstance(source_matrix, self.__class__):
result_mat_ref = result_mat_ref + source_matrix_ref
try:
result_mat_ref = result_mat_ref.toarray()
except AttributeError:
result_mat_ref = np.asarray(result_mat_ref)
elif isinstance(source_matrix, (np.ndarray, StateMatrixNumpy)):
result_mat_ref = result_mat_ref + source_matrix_ref
else:
type.mro(type(other))
print(type.mro(type(other)))
print(other)
raise TypeError(
"matrix has to be either 'StateMatrixNumpy', or 'StateMatrixSpSciPy', or 'np.ndarray' "
)
# raise TypeError("matrix has to be either 'StateMatrixNumpy', or 'StateMatrixSpSciPy'! ")
#
return result_mat
def solve(self, A, d, **kwargs):
"""Solves linear systems with a tridiagonal coefficient matrix.
A solver that uses the Thomas algorithm (the Tridiagonal matrix
algorithm) for systems on the form
0
[b c ] [x] [d]
A.x = [a b c ] [x] = [d] = d.
[ a b c] [x] [d]
[ a b] [x] [d]
0
Parameters
----------
A : A sparse diagonal matrix (dia format) with shape n-by-p. The
coefficient matrix.
b : Numpy array, n-by-1. The right-hand-side vector.
Examples
--------
>>> import numpy as np
>>> import scipy.sparse as sparse
>>> import parsimony.utils.linalgs as linalgs
>>> np.random.seed(42)
>>>
>>> n = 10
>>> a = np.random.rand(n); a[-1] = 0.0
>>> b = np.random.rand(n)
>>> c = np.random.rand(n); c[0] = 0.0
>>> abc = np.vstack((a, b, c))
>>> A = sparse.dia_matrix((abc, [-1, 0, 1]), shape=(n, n))
>>> d = np.random.rand(n, 1)
>>>
>>> solver = linalgs.TridiagonalSolver()
>>> x = solver.solve(A, d)
>>> print(x)
[[ -1.84339326]
[ 4.62737333]
[-12.41571989]
[ 16.38029815]
[ 14.38143172]
[-14.58969243]
[ 6.21233944]
[ 1.34271395]
[ -1.63358708]
[ 4.88318651]]
>>> x_ = np.linalg.solve(A.toarray(), d)
>>> print(x_)
[[ -1.84339326]
[ 4.62737333]
[-12.41571989]
[ 16.38029815]
[ 14.38143172]
[-14.58969243]
[ 6.21233944]
[ 1.34271395]
[ -1.63358708]
[ 4.88318651]]
>>> np.linalg.norm(x - x_) < 5e-14
True
>>>
>>> import time
>>> n = 100
>>> a = np.random.rand(n); a[-1] = 0.0
>>> b = np.random.rand(n)
>>> c = np.random.rand(n); c[0] = 0.0
>>> abc = np.vstack((a, b, c))
>>> A = sparse.dia_matrix((abc, [-1, 0, 1]), shape=(n, n))
>>> d = np.random.rand(n, 1)
>>>
>>> t = time.time()
>>> x = solver.solve(A, d)
>>> print "Time:", time.time() - t # doctest: +SKIP
>>>
>>> t = time.time()
>>> x_ = np.linalg.solve(A.toarray(), d)
>>> print "Time:", time.time() - t # doctest: +SKIP
>>>
>>> np.linalg.norm(x - x_) < 5e-12
True
>>>
>>> n = 1000
>>> a = np.random.rand(n); a[-1] = 0.0
>>> b = np.random.rand(n)
>>> c = np.random.rand(n); c[0] = 0.0
>>> abc = np.vstack((a, b, c))
>>> A = sparse.dia_matrix((abc, [-1, 0, 1]), shape=(n, n))
>>> d = np.random.rand(n, 1)
>>>
>>> t = time.time()
>>> x = solver.solve(A, d)
>>> print "Time:", time.time() - t # doctest: +SKIP
>>>
>>> t = time.time()
>>> x_ = np.linalg.solve(A.toarray(), d)
>>> print "Time:", time.time() - t # doctest: +SKIP
>>>
>>> np.linalg.norm(x - x_) < 5e-9
True
"""
# TODO: Put in compiled code for speed.
if not sparse.isspmatrix_dia(A):
A = A.todia()
abc = A.data
a = abc[0, :]
b = abc[1, :]
c = abc[2, :]
if abc.dtype != np.float:
a = np.asarray(a, np.float)
b = np.asarray(b, np.float)
c = np.asarray(c, np.float)
n = len(a)
x = np.zeros(n)
# Decomposition and forward substitution.
c_ = np.zeros(n)
d_ = np.zeros(n)
i = 0
if abs(b[i]) < consts.TOLERANCE:
# TODO: Do this instead: In this case x0 is found trivially and we
# recurse to a problem of order n-1.
solver = SparseSolver()
return solver.solve(A, d)
c_[i + 1] = c[i + 1] / b[i]
d_[i] = d[i] / b[i]
for i in range(1, n - 1):
i_1 = i - 1
den = (b[i] - a[i_1] * c_[i])
if abs(den) < consts.TOLERANCE: # We cannot handle this case!
# TODO: Use algorithm for banded matrices instead!
solver = SparseSolver()
return solver.solve(A, d)
c_[i + 1] = c[i + 1] / den
d_[i] = (d[i] - a[i_1] * d_[i_1]) / den
i = n - 1
d_[i] = (d[i] - a[i - 1] * d_[i - 1]) / (b[i] - a[i - 1] * c_[i])
# Back substitution.
i = n - 1
x[i] = d_[i]
for i in reversed(range(n - 1)):
x[i] = d_[i] - c_[i + 1] * x[i + 1]
return x.reshape((n, 1))
def __init__(self, A, b):
if not sparse.isspmatrix_dia(A) and not np.allclose(A, A.T):
raise ValueError('A should be a symmetric matrix.')
self.A = A
self.b = b
def solve(self, q, dq, dt):
"""Takes sensed q,dq, timestep dt and returns qdes and dqdes
in joint space.
"""
for task in self.taskList:
task.updateState(q, dq, dt)
# priority 1
if not hasattr(self, 'timingStats'):
self.timingStats = defaultdict(int)
self.timingStats['count'] += 1
t1 = time.time()
J1 = self.getStackedJacobian(q, dq, 1)
v1 = self.getStackedVelocity(q, dq, dt, 1)
(A, b) = self.getMotionModel(q, dq, dt)
if self.activeDofs != None:
A = select_cols(A, self.activeDofs)
if sparse.isspmatrix_coo(A) or sparse.isspmatrix_dia(A):
A = A.tocsr()
t2 = time.time()
self.timingStats['get jac/vel p1'] += t2 - t1
J2 = self.getStackedJacobian(q, dq, 2)
if J2 is not None:
V2 = self.getStackedVelocity(q, dq, dt, 2)
t3 = time.time()
self.timingStats['get jac/vel p2'] += t3 - t2
#compute velocity limits
vmax = self.robot.getVelocityLimits()
vmin = vectorops.mul(vmax, -1.0)
amax = self.robot.getAccelerationLimits()
vref = dq if self.ulast == None else self.ulast
for i, (v, vm, am) in enumerate(zip(vref, vmin, amax)):
if v - dt * am > vm:
vmin[i] = v - dt * am
elif v < vm:
#accelerate!
vmin[i] = min(vm, v + dt * am)
for i, (v, vm, am) in enumerate(zip(vref, vmax, amax)):
if v - dt * am < vm:
vmax[i] = v + dt * am
elif v > vm:
#decelerate!
vmax[i] = max(vm, v - dt * am)
for i, (l, u) in enumerate(zip(vmin, vmax)):
assert l <= u
if l > 0 or u < 0:
print "Moving link:", self.robot.getLink(
i).getName(), "speed", vref[i]
#print zip(vmin,vmax)
Aumin = np.array(vmin) - b
Aumax = np.array(vmax) - b
#print zip(Aumin.tolist(),Aumax.tolist())
J1A = J1.dot(A)
J1b = J1.dot(b)
if J2 == None:
#just solve constrained least squares
#J1*(A*u+b) = v1
#vmin < A*u + b < vmax
u1 = constrained_lsqr(J1A, v1 - J1b, A, Aumin, Aumax)[0]
u2 = [0.0] * len(u1)
t4 = time.time()
self.timingStats['pinv jac p1'] += t4 - t3
else:
#solve equality constrained least squares
#dq = A*u + b
#J1*dq = v1
#J1*A*u + J1*b = v1
#least squares solve for u1:
#J1*A*u1 = v1 - J1*b
#vmin < A*u1 + b < vmax
#need u to satisfy
#Aact*u = bact
#we know that u1 satisfies Aact*u = bact
#let u = u1+u2
#=> u2 = (I - Aact^+ Aact) z = N*z
#least squares solve for z:
#J2*A*(u1+u2+b) = v2
#J2*A*N z = v2 - J2*(A*u1+b)
(u1, active, activeRhs) = constrained_lsqr(J1A, v1 - J1b, A, Aumin,
Aumax)
Aact = sparse.vstack([J1A] + [A[crow, :]
for crow in active]).todense()
#bact = np.hstack((v1-J1b,activeRhs))
J1Ainv = np.linalg.pinv(Aact)
dq1 = A.dot(u1) + b
if len(active) > 0:
print "Priority 1 active constraints:"
for a in active:
print self.robot.getLink(
a).getName(), vmin[a], dq1[a], vmax[a]
r1 = J1.dot(dq1) - v1
print "Op space controller solve"
print " Residual 1", np.linalg.norm(r1)
# priority 2
N = np.eye(len(dq)) - np.dot(J1Ainv, Aact)
t4 = time.time()
self.timingStats['pinv jac p1'] += t4 - t3
u2 = [0.0] * len(u1)
#print " Initial priority 2 task error",np.linalg.norm(V2-J2.dot(dq1))
J2A = J2.dot(A)
J2AN = J2A.dot(N)
AN = sparse.csr_matrix(np.dot(A.todense(), N))
#Note: N destroys sparsity
V2_m_resid = np.ravel(V2 - J2.dot(dq1))
(z, active, activeRhs) = constrained_lsqr(J2AN, V2_m_resid, AN,
vmin - dq1, vmax - dq1)
t5 = time.time()
self.timingStats['ls jac p2'] += t5 - t4
u2 = np.ravel(np.dot(N, z))
#debug, should be close to zero
#print " Nullspace projection error:",np.linalg.norm(J1A.dot(u2))
#this is the error in the nullspace of the first priority tasks
dq2 = A.dot(u2) + dq1
#debug, should be equal to residual 2 printout above
print " Residual 2", np.linalg.norm(J2.dot(dq2) - V2)
#debug should be close to zero
#print " Residual 2 in priority 1 frame",np.linalg.norm(J1.dot(dq2)-v1)
if len(active) > 0:
print "Priority 2 active constraints:"
for a in active:
print self.robot.getLink(
a).getName(), vmin[a], dq2[a], vmax[a]
#compose the velocities together
u = np.ravel((u1 + u2))
dqpred = A.dot(u) + b
print " Residual 1 final", np.linalg.norm(
np.ravel(J1.dot(dqpred)) - v1)
if J2 != None:
print " Residual 2 final", np.linalg.norm(
np.ravel(J2.dot(dqpred)) - V2)
u = u.tolist()
#if self.activeDofs != None:
# print "dqdes:",[self.dqdes[v] for v in self.activeDofs]
self.qdes = vectorops.madd(q, u, dt)
self.ulast = u
t6 = time.time()
self.timingStats['total'] += t6 - t1
if self.timingStats['count'] % 10 == 0:
n = self.timingStats['count']
print "OpSpace times (ms): vel/jac 1 %.2f inv 1 %.2f vel/jac 2 %.2f inv 2 %.2f total %.2f" % (
self.timingStats['get jac/vel p1'] / n * 1000,
self.timingStats['pinv jac p1'] / n * 1000,
self.timingStats['get jac/vel p2'] / n * 1000,
self.timingStats['ls jac p2'] / n * 1000,
self.timingStats['total'] / n * 1000)
return (self.qdes, u)
def constrained_lsqr(A, b, C, p, q):
"""Solves the least-squares problem min_x ||Ax-b||^2
with the constraints p<=Cx<=q. If there are more than one solution,
picks the one with the lowest L-2 norm.
The result is (x,activeSet,activeRhs) where activeSet lists the
indices of the bounds that are met exactly and activeRhs lists
the values.
"""
(x, istop, itn, normr, normar, norma, conda,
normx) = sparse.linalg.lsmr(A, b, damp=1e-5)
Cx = spdot(C, x)
pmax, ipmax = max((pi - xi, i) for i, (pi, xi) in enumerate(zip(p, Cx)))
qmax, iqmax = max((xi - qi, i) for i, (qi, xi) in enumerate(zip(q, Cx)))
candidates = set(range(A.shape[1]))
activeSet = []
activeRhs = []
AtA = None
while pmax > 0 or qmax > 0:
#add the most violated constraint to the active set
#and re-solve
cval = q[iqmax]
crow = iqmax
if pmax > qmax:
cval = p[ipmax]
crow = ipmax
#print "Bound %d violation %f <= %f <= %f, active set size %d"%(crow,p[crow],Cx[crow],q[crow],len(activeSet))
candidates.remove(crow)
activeSet.append(crow)
activeRhs.append(cval)
#form active set matrices
if sparse.isspmatrix_coo(C) or sparse.isspmatrix_dia(C):
C = C.tocsr()
Atemp = sparse.vstack([A] + [C[crow, :] for crow in activeSet])
btemp = np.hstack((b, activeRhs))
#solve for A'x ~= b' with starting point x0 that satisfies Ax=b
#for old active set
#Let x=y+x0
#Solve for y that solves A'y ~= b'-A'x0
Atempx = Atemp.dot(x)
(y, istop, itn, normr, normar, norma, conda,
normx) = sparse.linalg.lsmr(Atemp, btemp - Atempx)
if normr > 1e-4:
#solve (AtA)x+C^T z = At b, Cx=d
if AtA == None:
AtA = A.T.dot(A)
Ca = sparse.vstack([C[crow, :] for crow in activeSet])
kktMatrix = sparse.vstack(
sparse.hstack((AtA, Ca.T)),
sparse.hstack((Ca, sparse.csr_matrix(
(Ca.shape[0], Ca.shape[0])))))
(xz, istop, itn, normr, normar, norma, conda,
normx) = sparse.linalg.lsmr(
kktMatrix, np.hstack((np.dot(A.T, b), activeRhs)))
if normr > 1e-4:
print "Warning, could not solve for constraints exactly, error", normr
x = xz[:x.shape[0]]
else:
x = x + y
#(x,istop,itn,normr,normar,norma,conda,normx) = sparse.linalg.lsmr(Atemp,btemp)
Cx = spdot(C, x)
pmax, ipmax = max((p[i] - Cx[i], i) for i in candidates)
qmax, iqmax = max((Cx[i] - q[i], i) for i in candidates)
chtol = 1e-7
for i in xrange(len(x)):
if Cx[i] < p[i] - chtol or Cx[i] > q[i] + chtol:
print "Warning, constraint violation %d: %f <= %f <= %f" % (
i, p[i], Cx[i], q[i])
return (x, activeSet, activeRhs)
