def ikSolver(self, robot, obj_pt, obj_axis):
"""Returns an IK solver that places the hand center at obj_pt with
the palm oriented along obj_axis"""
q = robot.getConfig()
obj = IKObjective()
obj.setFixedPoints(self.link,
[self.localPosition1, self.localPosition2], [
vectorops.madd(obj_pt, obj_axis, -0.03),
vectorops.madd(obj_pt, obj_axis, 0.03)
])
solver = IKSolver(robot)
solver.add(obj)
solver.setActiveDofs(self.armIndices)
return solver
def getCommandVelocity(self, q, dq, dt):
"""Gets the command velocity from the current state of the
robot.
"""
eP = self.getSensedError(q)
#vcmd = hP*eP + hD*eV + hI*eI
vP = gen_mul(self.hP, eP)
vcmd = vP
#D term
vcur = self.getSensedVelocity(q, dq, dt)
eD = None
if vcur != None:
eD = vectorops.sub(vcur, self.dxdes)
vD = gen_mul(self.hD, eD)
vcmd = vectorops.add(vcmd, vD)
#I term
if self.eI != None:
vI = gen_mul(self.hI, self.eI)
vcmd = vectorops.add(vcmd, vI)
#print "task",self.name,"error P=",eP,"D=",eD,"E=",self.eI
#do acceleration limiting
if vcur != None:
dvcmd = vectorops.div(vectorops.sub(vcmd, vcur), dt)
dvnorm = vectorops.norm(dvcmd)
if dvnorm > self.dvcmdmax:
vcmd = vectorops.madd(vcur, dvcmd, self.dvcmdmax / dvnorm * dt)
print self.name, "acceleration limited by factor", self.dvcmdmax / dvnorm * dt, "to", vcmd
#do velocity limiting
vnorm = vectorops.norm(vcmd)
if vnorm > self.vcmdmax:
vcmd = vectorops.mul(vcmd, self.vcmdmax / vnorm)
print self.name, "velocity limited by factor", self.vcmdmax / vnorm, "to", vcmd
return vcmd
def getCommandVelocity(self, q, dq, dt): """Gets the command velocity from the current state of the robot. """ eP = self.getSensedError(q) #vcmd = hP*eP + hD*eV + hI*eI vP = gen_mul(self.hP,eP) vcmd = vP #D term vcur = self.getSensedVelocity(q,dq,dt) eD = None if vcur != None: eD = vectorops.sub(vcur, self.dxdes) vD = gen_mul(self.hD,eD) vcmd = vectorops.add(vcmd, vD) #I term if self.eI != None: vI = gen_mul(self.hI,self.eI) vcmd = vectorops.add(vcmd, vI) #print "task",self.name,"error P=",eP,"D=",eD,"E=",self.eI #do acceleration limiting if vcur != None: dvcmd = vectorops.div(vectorops.sub(vcmd,vcur),dt) dvnorm = vectorops.norm(dvcmd) if dvnorm > self.dvcmdmax: vcmd = vectorops.madd(vcur,dvcmd,self.dvcmdmax/dvnorm*dt) print self.name,"acceleration limited by factor",self.dvcmdmax/dvnorm*dt,"to",vcmd #do velocity limiting vnorm = vectorops.norm(vcmd) if vnorm > self.vcmdmax: vcmd = vectorops.mul(vcmd,self.vcmdmax/vnorm) print self.name,"velocity limited by factor",self.vcmdmax/vnorm,"to",vcmd return vcmd
def process(self, inputs):
if self.value == None or len(self.value) != len(inputs):
self.value = inputs
else:
x = vectorops.mul(self.value, 1 - self.rate)
self.value = vectorops.madd(x, inputs, self.rate)
return self.value
def append_debounced_command(qend,vend,qdes,motion_queue):
"""Appends a debounced command from qend,vend to qdes,vdes (with vdes=0)
to the given motion queue. Assumes qend,vend is the end of the motion
queue."""
global vmax
#hack: estimate time of motion
L = Linf_norm(vend)
p = vectorops.madd(qend,vend,0.5)
L += Linf_norm(vectorops.sub(qdes,p))
T = math.sqrt(L / vmax)
if T == 0:
return
temp = vectorops.madd(qend,vectorops.sub(qdes,qend),0.85)
motion_queue.appendCubic(T,temp,vectorops.mul(vectorops.sub(qdes,qend),1.0/T*0.25))
temp2 = vectorops.madd(qend,vectorops.sub(qdes,qend),0.95)
motion_queue.appendCubic(T*0.75,temp2,vectorops.mul(vectorops.sub(qdes,qend),1.0/T*0.05))
motion_queue.appendCubic(T*0.5,qdes,[0]*len(qdes))
return
def process(self,inputs):
#do a filter
if self.value == None:
self.value = inputs
else:
#exponential filter
x = vectorops.mul(self.value,1.0-self.rate)
self.value = vectorops.madd(x,inputs,self.rate)
return self.value
def process(self, inputs):
#do a filter
if self.value == None:
self.value = inputs
else:
#exponential filter
x = vectorops.mul(self.value, 1.0 - self.rate)
self.value = vectorops.madd(x, inputs, self.rate)
return self.value
def ikSolver(self,robot,obj_pt,obj_axis):
"""Returns an IK solver that places the hand center at obj_pt with
the palm oriented along obj_axis"""
q = robot.getConfig()
obj = IKObjective()
obj.setFixedPoints(self.link,[self.localPosition1,self.localPosition2],[vectorops.madd(obj_pt,obj_axis,-0.03),vectorops.madd(obj_pt,obj_axis,0.03)])
solver = IKSolver(robot)
solver.add(obj)
solver.setActiveDofs(self.armIndices)
return solver
def send_debounced_command(qcur,vcur,qdes,motion_queue):
"""Sends a debounced command from qcur,vcur to qdes,vdes (with vdes=0)
to the given motion queue"""
global vmax
#hack: estimate time of motion
L = Linf_norm(vcur)
p = vectorops.madd(qcur,vcur,0.5)
L += Linf_norm(vectorops.sub(qdes,p))
T = math.sqrt(L / vmax)
if T == 0:
return
temp = vectorops.madd(qcur,vectorops.sub(qdes,qcur),0.85)
motion_queue.setCubic(T,temp,vectorops.mul(vectorops.sub(qdes,qcur),1.0/T*0.25))
temp2 = vectorops.madd(qcur,vectorops.sub(qdes,qcur),0.95)
motion_queue.appendCubic(T*0.75,temp2,vectorops.mul(vectorops.sub(qdes,qcur),1.0/T*0.05))
motion_queue.appendCubic(T*0.5,qdes,[0]*len(qdes))
return
#old: trying quintic motion
"""
def nextState(self, x, u):
pos = [x[0], x[1]]
fwd = [math.cos(x[2]), math.sin(x[2])]
right = [-fwd[1], fwd[0]]
phi = u[1]
d = u[0]
if abs(phi) < 1e-8:
newPos = vectorops.madd(pos, fwd, d)
return newpos + [x[2]]
else:
#rotate about a center of rotation, with radius 1/phi
cor = vectorops.madd(pos, right, 1.0 / phi)
sign = cmp(d * phi, 0)
d = abs(d)
phi = abs(phi)
theta = 0
thetaMax = d * phi
newpos = vectorops.add(
so2.apply(sign * thetaMax, vectorops.sub(pos, cor)), cor)
return newpos + [so2.normalize(x[2] + sign * thetaMax)]
def drawGL(self):
#draw tendons
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
glLineWidth(4.0)
glColor3f(0, 1, 1)
glBegin(GL_LINES)
for i in range(3):
b1 = self.sim.body(
self.model.robot.link(self.model.proximal_links[i]))
b2 = self.sim.body(
self.model.robot.link(self.model.distal_links[i]))
glVertex3f(*se3.apply(b1.getTransform(), self.tendon0_local))
glVertex3f(*se3.apply(b1.getTransform(), self.tendon1_local))
glVertex3f(*se3.apply(b1.getTransform(), self.tendon1_local))
glVertex3f(*se3.apply(b2.getTransform(), self.tendon2_local))
glEnd()
glLineWidth(1)
glColor3f(1, 0.5, 0)
glBegin(GL_LINES)
fscale = 0.01
for i in range(3):
b1 = self.sim.body(
self.model.robot.link(self.model.proximal_links[i]))
b2 = self.sim.body(
self.model.robot.link(self.model.distal_links[i]))
if self.forces[i][0] != None:
p = se3.apply(b1.getTransform(), self.tendon0_local)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p, self.forces[i][0], fscale))
if self.forces[i][1] != None:
p = se3.apply(b1.getTransform(), self.tendon1_local)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p, self.forces[i][1], fscale))
if self.forces[i][2] != None:
p = se3.apply(b2.getTransform(), self.tendon2_local)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p, self.forces[i][2], fscale))
glEnd()
glEnable(GL_DEPTH_TEST)
glEnable(GL_LIGHTING)
def draw_GL_frame(T,axis_length=0.1):
"""Draws the rigid transform T as a set of axis-oriented lines
of length axis_length."""
R,t = T
if len(R)!=9 or len(t)!=3:
print "Incorrect sizes",len(R),len(t)
raise RuntimeError("")
#draw transform widget
glBegin(GL_LINES)
glColor3f(1,1,1)
glVertex3fv(t)
glColor3f(1,0,0)
glVertex3fv(vectorops.madd(t,R[0:3],axis_length))
glColor3f(1,1,1)
glVertex3fv(t)
glColor3f(0,1,0)
glVertex3fv(vectorops.madd(t,R[3:6],axis_length))
glColor3f(1,1,1)
glVertex3fv(t)
glColor3f(0,0,1)
glVertex3fv(vectorops.madd(t,R[6:9],axis_length))
glEnd()
glColor3f(0,0,0)
def draw_GL_frame(T, axis_length=0.1):
"""Draws the rigid transform T as a set of axis-oriented lines
of length axis_length."""
R, t = T
if len(R) != 9 or len(t) != 3:
print "Incorrect sizes", len(R), len(t)
raise RuntimeError("")
#draw transform widget
glBegin(GL_LINES)
glColor3f(1, 1, 1)
glVertex3fv(t)
glColor3f(1, 0, 0)
glVertex3fv(vectorops.madd(t, R[0:3], axis_length))
glColor3f(1, 1, 1)
glVertex3fv(t)
glColor3f(0, 1, 0)
glVertex3fv(vectorops.madd(t, R[3:6], axis_length))
glColor3f(1, 1, 1)
glVertex3fv(t)
glColor3f(0, 0, 1)
glVertex3fv(vectorops.madd(t, R[6:9], axis_length))
glEnd()
glColor3f(0, 0, 0)
def advance(self, q, dq, dt):
""" Updates internal state: accumulates iterm and updates x_last
"""
if self.weight > 0:
eP = self.getSensedError(q)
# update iterm
if self.eI == None:
self.eI = vectorops.mul(eP, dt)
else:
self.eI = vectorops.madd(self.eI, eP, dt)
einorm = vectorops.norm(self.eI)
if einorm > self.eImax:
self.eI = vectorops.mul(self.eI, self.eImax / einorm)
#update qLast
self.qLast = q
return
def advance(self, q, dq, dt): """ Updates internal state: accumulates iterm and updates x_last """ if self.weight > 0: eP = self.getSensedError(q) # update iterm if self.eI == None: self.eI = vectorops.mul(eP,dt) else: self.eI = vectorops.madd(self.eI, eP, dt) einorm = vectorops.norm(self.eI) if einorm > self.eImax: self.eI = vectorops.mul(self.eI,self.eImax/einorm) #update qLast self.qLast = q return
def apply_tendon_forces(self, i, link1, link2, rest_length):
tendon_c2 = 30000.0
tendon_c1 = 10000.0
b0 = self.sim.body(
self.model.robot.link(self.model.proximal_links[0] - 3))
b1 = self.sim.body(self.model.robot.link(link1))
b2 = self.sim.body(self.model.robot.link(link2))
p0w = se3.apply(b1.getTransform(), self.tendon0_local)
p1w = se3.apply(b1.getTransform(), self.tendon1_local)
p2w = se3.apply(b2.getTransform(), self.tendon2_local)
d = vectorops.distance(p1w, p2w)
if d > rest_length:
#apply tendon force
direction = vectorops.unit(vectorops.sub(p2w, p1w))
f = tendon_c2 * (d - rest_length)**2 + tendon_c1 * (d -
rest_length)
#print d,rest_length
#print "Force magnitude",self.model.robot.link(link1).getName(),":",f
f = min(f, 100)
#tendon routing force
straight = vectorops.unit(vectorops.sub(p2w, p0w))
pulley_direction = vectorops.unit(vectorops.sub(p1w, p0w))
pulley_axis = so3.apply(b1.getTransform()[0], (0, 1, 0))
tangential_axis = vectorops.cross(pulley_axis, pulley_direction)
cosangle = vectorops.dot(straight, tangential_axis)
#print "Cosine angle",self.model.robot.link(link1).getName(),cosangle
base_direction = so3.apply(b0.getTransform()[0], [0, 0, -1])
b0.applyForceAtLocalPoint(
vectorops.mul(base_direction, -f),
vectorops.madd(p0w, base_direction, 0.04))
b1.applyForceAtLocalPoint(
vectorops.mul(tangential_axis, cosangle * f),
self.tendon1_local)
b1.applyForceAtLocalPoint(
vectorops.mul(tangential_axis, -cosangle * f),
self.tendon0_local)
b2.applyForceAtLocalPoint(vectorops.mul(direction, -f),
self.tendon2_local)
self.forces[i][1] = vectorops.mul(tangential_axis, cosangle * f)
self.forces[i][2] = vectorops.mul(direction, f)
else:
self.forces[i] = [None, None, None]
return
def control_loop(self):
sim = self.sim
world = self.world
controller = sim.getController(0)
robotModel = world.robot(0)
t = self.ttotal
#Problem 1: tune the values in this line
if problem == "1a" or problem == "1b":
kP = 1
kI = 1
kD = 1
controller.setPIDGains([kP], [kI], [kD])
#Problem 2: use setTorque to implement a control loop with feedforward
#torques
if problem == "2a" or problem == "2b":
qcur = controller.getSensedConfig()
dqcur = controller.getSensedVelocity()
qdes = [0]
dqdes = [0]
robotModel.setConfig(qcur)
robotModel.setVelocity(dqcur)
while qcur[0] - qdes[0] > math.pi:
qcur[0] -= 2 * math.pi
while qcur[0] - qdes[0] < -math.pi:
qcur[0] += 2 * math.pi
ddq = vectorops.mul(vectorops.sub(qdes, qcur), 100.0)
ddq = vectorops.madd(ddq, vectorops.sub(dqdes, dqcur), 20.0)
#TODO: solve the dynamics equation to fill this in
T = [3]
controller.setTorque(T)
#Problem 3: drive the robot so its end effector goes
#smoothly toward the target point
if problem == "3":
target = [1.0, 0.0, 0.0]
qdes = [0.7, -2.3]
dqdes = [0, 0]
controller.setPIDCommand(qdes, dqdes)
def control_loop(self):
sim = self.sim
world = self.world
controller = sim.getController(0)
robotModel = world.robot(0)
t = self.ttotal
#Problem 1: tune the values in this line
if problem == "1a" or problem == "1b":
kP = 1
kI = 1
kD = 1
controller.setPIDGains([kP],[kI],[kD])
#Problem 2: use setTorque to implement a control loop with feedforward
#torques
if problem == "2a" or problem == "2b":
qcur = controller.getSensedConfig()
dqcur = controller.getSensedVelocity()
qdes = [0]
dqdes = [0]
robotModel.setConfig(qcur)
robotModel.setVelocity(dqcur)
while qcur[0]-qdes[0] > math.pi:
qcur[0] -= 2*math.pi
while qcur[0]-qdes[0] < -math.pi:
qcur[0] += 2*math.pi
ddq = vectorops.mul(vectorops.sub(qdes,qcur),100.0)
ddq = vectorops.madd(ddq,vectorops.sub(dqdes,dqcur),20.0)
#TODO: solve the dynamics equation to fill this in
T = [3]
controller.setTorque(T)
#Problem 3: drive the robot so its end effector goes
#smoothly toward the target point
if problem == "3":
target = [1.0,0.0,0.0]
qdes = [0.7,-2.3]
dqdes = [0,0]
controller.setPIDCommand(qdes,dqdes)
def interpolate(a, b, u):
"""Interpolates linearly between a and b"""
return vectorops.madd(a, vectorops.sub(b, a), u)
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 interpolate_linear(a, b, u):
"""Interpolates linearly in cartesian space between a and b."""
return vectorops.madd(a, vectorops.sub(b, a), u)
def interpolate_linear(a,b,u):
"""Interpolates linearly in cartesian space between a and b."""
return vectorops.madd(a,vectorops.sub(b,a),u)
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 interpolate(a,b,u):
"""Interpolates linearly between a and b"""
return vectorops.madd(a,vectorops.sub(b,a),u)
