parameters: (self, q,dq,dt)
return_statement: return (self.qdes,u)
docstring: Takes sensed q,dq, timestep dt and returns qdes and dqdes
  		in joint space.
docstring_summary: Takes sensed q,dq, timestep dt and returns qdes and dqdes
  		in joint space.
docstring_tokens: Takes sensed q dq timestep dt and returns qdes and dqdes in joint space .
function: 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.link(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 self.verbose and len(active)>0:
  				print "Priority 1 active constraints:"
  				for a in active:
  					print self.robot.link(a).getName(),vmin[a],dq1[a],vmax[a]
  
  			r1 = J1.dot(dq1)-v1
  			if self.verbose:
  				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
  			if self.verbose:
  				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 self.verbose and len(active)>0:
  				print "Priority 2 active constraints:"
  				for a in active:
  					print self.robot.link(a).getName(),vmin[a],dq2[a],vmax[a]
  
  		#compose the velocities together
  		u = np.ravel((u1 + u2))
  		dqpred = A.dot(u)+b
  		if self.verbose:
  			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.verbose:
  			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)
function_tokens: def solve ( self , q , dq , dt ) : 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 . link ( 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 self . verbose and len ( active ) > 0 : print "Priority 1 active constraints:" for a in active : print self . robot . link ( a ) . getName ( ) , vmin [ a ] , dq1 [ a ] , vmax [ a ] r1 = J1 . dot ( dq1 ) - v1 if self . verbose : 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 if self . verbose : 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 self . verbose and len ( active ) > 0 : print "Priority 2 active constraints:" for a in active : print self . robot . link ( a ) . getName ( ) , vmin [ a ] , dq2 [ a ] , vmax [ a ] #compose the velocities together u = np . ravel ( ( u1 + u2 ) ) dqpred = A . dot ( u ) + b if self . verbose : 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 . verbose : 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 )