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)
Beispiel #2
0
 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))
Beispiel #3
0
 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)
Beispiel #4
0
    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)
Beispiel #5
0
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)')
Beispiel #6
0
 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]
Beispiel #7
0
 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)
Beispiel #8
0
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
Beispiel #10
0
    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)
Beispiel #11
0
 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)
Beispiel #14
0
    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
Beispiel #16
0
    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))
Beispiel #17
0
 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
Beispiel #18
0
    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)
Beispiel #19
0
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)