def gen_mul(a,b): """Multiplies lists or scalars in a unified way. With lists the multiplication is elementwise.""" if isinstance(a,np.ndarray): return np.dot(a,b) elif hasattr(a,'__iter__'): return vectorops.mul(a,b) elif hasattr(b,'__iter__'): return vectorops.mul(b,a) return a*b
def gen_mul(a, b):
"""Multiplies lists or scalars in a unified way. With lists
the multiplication is elementwise."""
if isinstance(a, np.ndarray):
return np.dot(a, b)
elif hasattr(a, '__iter__'):
return vectorops.mul(a, b)
elif hasattr(b, '__iter__'):
return vectorops.mul(b, a)
return a * b
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 spawn_item_in_world(item,world):
"""Given an ItemInBin instance, spawns a RigidObject in the Klamp't
world with its same geometry / size / mass properties.
Returns the new object."""
obj = world.makeRigidObject(item.info.name)
bmin,bmax = item.info.bmin,item.info.bmax
center = vectorops.div(vectorops.add(bmin,bmax),2.0)
m = obj.getMass()
m.setMass(item.info.mass)
m.setCom([0,0,0])
m.setInertia(vectorops.mul([bmax[0]-bmin[0],bmax[1]-bmin[1],bmax[2]-bmin[2]],item.info.mass/12.0))
obj.setMass(m)
c = obj.getContactParameters()
#TODO: make these parameters dynamic
c.kFriction = 0.6
c.kRestitution = 0.1;
c.kStiffness = 100000
c.kDamping = 100000
obj.setContactParameters(c)
simgeometry = obj.geometry()
#either copy directly (this line)...
simgeometry.set(item.info.geometry)
#or reload from file (this line)
#load_item_geometry(item.info,simgeometry)
obj.setTransform(item.xform[0],item.xform[1])
return obj
def scaleToBounds( val, bmin, bmax, origin=None): """Cap the ray starting origin in the direction val against bounding box limits. """ if origin != None: if isinstance(bmin,np.ndarray): bmin -= origin else: bmin = vectorops.sub(bmin,origin) if isinstance(bmax,np.ndarray): bmax -= origin else: bmax = vectorops.sub(bmax,origin) return scaleToBounds(val,bmin,bmax) umax = 1.0 for vali,ai,bi in zip(val,bmin,bmax): assert bi >= ai if vali < ai: umax = ai/vali if vali > bi: umax = bi/vali if umax==1.0: return val assert umax >= 0 print "Scaling to velocity maximum, factor",umax if isinstance(val,np.ndarray): return val*umax else: return vectorops.mul(val,umax)
def putSensedObjectInWorld(self,item):
#put this obejct in the world
obj = self.world.makeRigidObject(item.info.name)
bmin,bmax = item.info.bmin,item.info.bmax
center = vectorops.div(vectorops.add(bmin,bmax),2.0)
m = obj.getMass()
m.setMass(item.info.mass)
m.setCom([0,0,0])
m.setInertia(vectorops.mul([bmax[0]-bmin[0],bmax[1]-bmin[1],bmax[2]-bmin[2]],item.info.mass/12.0))
obj.setMass(m)
c = obj.getContactParameters()
c.kFriction = 0.6
c.kRestitution = 0.1;
c.kStiffness = 100000
c.kDamping = 100000
obj.setContactParameters(c)
cube = obj.geometry()
if not cube.loadFile(os.path.join(model_dir,"cube.tri")):
print "Error loading cube file",os.path.join(model_dir,"cube.tri")
exit(1)
scale = [bmax[0]-bmin[0],0,0,0,bmax[1]-bmin[1],0,0,0,bmax[2]-bmin[2]]
translate = vectorops.sub(bmin,center)
cube.transform(scale,translate)
mesh = cube.getTriangleMesh()
obj.setTransform(item.xform[0],item.xform[1])
def load_item_geometry(item,geometry_ptr = None):
"""Loads the geometry of the given item and returns it. If geometry_ptr
is provided, then it is assumed to be a Geometry3D object and the object
geometry is loaded into it."""
if geometry_ptr == None:
geometry_ptr = Geometry3D()
if item.info.geometryFile == None:
return None
elif item.info.geometryFile == 'box':
fn = "../klampt_models/cube.tri"
if not geometry_ptr.loadFile(fn):
print "Error loading cube file",fn
exit(1)
bmin,bmax = item.info.bmin,item.info.bmax
center = vectorops.mul(vectorops.add(bmin,bmax),0.5)
scale = [bmax[0]-bmin[0],0,0,0,bmax[1]-bmin[1],0,0,0,bmax[2]-bmin[2]]
translate = vectorops.sub(bmin,center)
geometry_ptr.transform(scale,translate)
geometry_ptr.setCurrentTransform(item.xform[0],item.xform[1])
return geometry_ptr
else:
if not geometry_ptr.loadFile(item.info.geometryFile):
print "Error loading geometry file",item.info.geometryFile
exit(1)
return geometry_ptr
def printStatus(self,q):
"""Prints a status printout summarizing all tasks' errors."""
priorities = set()
names = dict()
errors = dict()
totalerrors = dict()
for t in self.taskList:
if t.weight==0: continue
priorities.add(t.level)
s = t.name
if len(s) > 8:
s = s[0:8]
err = t.getSensedError(q)
names.setdefault(t.level,[]).append(s)
errors.setdefault(t.level,[]).append("%.3f"%(vectorops.norm(err)),)
werrsq = vectorops.normSquared(vectorops.mul(err,t.weight))
totalerrors[t.level] = totalerrors.get(t.level,0.0) + werrsq
cols = 5
colwidth = 10
for p in priorities:
print "Priority",p,"weighted error^2",totalerrors[p]
pnames = names[p]
perrs = errors[p]
start = 0
while start < len(pnames):
last = min(start+cols,len(pnames))
print " Name: ",
for i in range(start,last):
print pnames[i],' '*(colwidth-len(pnames[i])),
print
print " Error: ",
for i in range(start,last):
print perrs[i],' '*(colwidth-len(perrs[i])),
print
start=last
def spawn_objects(world):
"""For all ground_truth_items, spawns RigidObjects in the world
according to their sizes / mass properties"""
print "Initializing world objects"
obj = world.makeRigidObject('object1')
bmin = [0,0,0]
bmax = [0.08,0.08,0.3]
mass = 0.001
m = obj.getMass()
m.setMass(mass)
m.setCom([0,0,0])
m.setInertia(vectorops.mul([bmax[0]-bmin[0],bmax[1]-bmin[1],bmax[2]-bmin[2]],mass/12.0))
obj.setMass(m)
c = obj.getContactParameters()
c.kFriction = 10
c.kRestitution = 0.1;
c.kStiffness = 100
c.kDamping = 10
obj.setContactParameters(c)
load_item_geometry(bmin,bmax,obj.geometry())
obj.setTransform(so3.identity(),[0,-0.2,0.155])
obj.geometry().setCollisionMargin(0)
return obj
def spawn_objects_from_ground_truth(world):
"""For all ground_truth_items, spawns RigidObjects in the world
according to their sizes / mass properties"""
global ground_truth_items
print "Initializing world objects"
for item in ground_truth_items:
obj = world.makeRigidObject(item.info.name)
bmin,bmax = item.info.bmin,item.info.bmax
center = vectorops.div(vectorops.add(bmin,bmax),2.0)
m = obj.getMass()
m.setMass(item.info.mass)
m.setCom([0,0,0])
m.setInertia(vectorops.mul([bmax[0]-bmin[0],bmax[1]-bmin[1],bmax[2]-bmin[2]],item.info.mass/12.0))
obj.setMass(m)
c = obj.getContactParameters()
c.kFriction = 0.6
c.kRestitution = 0.1;
c.kStiffness = 100000
c.kDamping = 100000
obj.setContactParameters(c)
cube = obj.geometry()
if not cube.loadFile(os.path.join(model_dir,"cube.tri")):
print "Error loading cube file",os.path.join(model_dir,"cube.tri")
exit(1)
scale = [bmax[0]-bmin[0],0,0,0,bmax[1]-bmin[1],0,0,0,bmax[2]-bmin[2]]
translate = vectorops.sub(bmin,center)
cube.transform(scale,translate)
mesh = cube.getTriangleMesh()
obj.setTransform(item.xform[0],item.xform[1])
return
def printStatus(self,q):
"""Prints a status printout summarizing all tasks' errors."""
priorities = set()
names = dict()
errors = dict()
totalerrors = dict()
for t in self.taskList:
if t.weight==0: continue
priorities.add(t.level)
s = t.name
if len(s) > 8:
s = s[0:8]
err = t.getSensedError(q)
names.setdefault(t.level,[]).append(s)
errors.setdefault(t.level,[]).append("%.3f"%(vectorops.norm(err)),)
werrsq = vectorops.normSquared(vectorops.mul(err,t.weight))
totalerrors[t.level] = totalerrors.get(t.level,0.0) + werrsq
cols = 5
colwidth = 10
for p in priorities:
print "Priority",p,"weighted error^2",totalerrors[p]
pnames = names[p]
perrs = errors[p]
start = 0
while start < len(pnames):
last = min(start+cols,len(pnames))
print " Name: ",
for i in range(start,last):
print pnames[i],' '*(colwidth-len(pnames[i])),
print
print " Error: ",
for i in range(start,last):
print perrs[i],' '*(colwidth-len(perrs[i])),
print
start=last
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 scaleToBounds(val, bmin, bmax, origin=None):
"""Cap the ray starting origin in the direction val against bounding box limits.
"""
if origin != None:
if isinstance(bmin, np.ndarray):
bmin -= origin
else:
bmin = vectorops.sub(bmin, origin)
if isinstance(bmax, np.ndarray):
bmax -= origin
else:
bmax = vectorops.sub(bmax, origin)
return scaleToBounds(val, bmin, bmax)
umax = 1.0
for vali, ai, bi in zip(val, bmin, bmax):
assert bi >= ai
if vali < ai:
umax = ai / vali
if vali > bi:
umax = bi / vali
if umax == 1.0: return val
assert umax >= 0
print "Scaling to velocity maximum, factor", umax
if isinstance(val, np.ndarray):
return val * umax
else:
return vectorops.mul(val, umax)
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,link1,link2,rest_length):
tendon_c2 = 30000.0
tendon_c1 = 100000.0
b1 = self.sim.body(self.model.robot.link(link1))
b2 = self.sim.body(self.model.robot.link(link2))
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)
b1.applyForceAtPoint(vectorops.mul(direction,f),p1w)
b2.applyForceAtPoint(vectorops.mul(direction,-f),p2w)
else:
pass
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 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 apply_tendon_forces(self,link1,link2,rest_length):
tendon_c2 = 30000.0
tendon_c1 = 100000.0
b1 = self.sim.getBody(self.model.robot.getLink(link1))
b2 = self.sim.getBody(self.model.robot.getLink(link2))
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",d,"rest length",rest_length,"force",f
b1.applyForceAtPoint(vectorops.mul(direction,f),p1w)
b2.applyForceAtPoint(vectorops.mul(direction,-f),p2w)
else:
#print "d",d,"rest length",rest_length
pass
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 getStackedVelocity(self, q, dq, dt, priority): """Formulates dx to calculate dqdes """ Vlist = [] for taski in self.taskList: if taski.weight == 0 or taski.level != priority: continue #scale by weight Vtemp = vectorops.mul(taski.getCommandVelocity(q, dq, dt),taski.weight) Vlist.append(Vtemp) if len(Vlist)==0: return None V = np.hstack(Vlist) return V
def getStackedVelocity(self, q, dq, dt, priority): """Formulates dx to calculate dqdes """ Vlist = [] for taski in self.taskList: if taski.weight == 0 or taski.level != priority: continue #scale by weight Vtemp = vectorops.mul(taski.getCommandVelocity(q, dq, dt),taski.weight) Vlist.append(Vtemp) if len(Vlist)==0: return None V = np.hstack(Vlist) return V
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 load_item_geometry(bmin,bmax,geometry_ptr = None):
"""Loads the geometry of the given item and returns it. If geometry_ptr
is provided, then it is assumed to be a Geometry3D object and the object
geometry is loaded into it."""
if geometry_ptr == None:
geometry_ptr = Geometry3D()
# fn = model_dir + "cylinder.tri"
fn = model_dir + "cube.tri"
# fn = model_dir + "/objects/teapot/pots.tri"
# bmin = [0,0,0]
# bmax = [1.2,1.5,1.25]
# fn = model_dir + "items/rollodex_mesh_collection_jumbo_pencil_cup/textured_meshes/optimized_poisson_textured_mesh.ply"
if not geometry_ptr.loadFile(fn):
print "Error loading cube file",fn
exit(1)
center = vectorops.mul(vectorops.add(bmin,bmax),0.5)
scale = [bmax[0]-bmin[0],0,0,0,bmax[1]-bmin[1],0,0,0,bmax[2]-bmin[2]]
translate = vectorops.sub(bmin,center)
geometry_ptr.transform(so3.mul(scale,so3.rotation([0,0,1],math.pi/180*0)),translate)
geometry_ptr.setCurrentTransform(so3.identity(),[0,0,0])
return geometry_ptr
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 move_to_grasp_object(self,object):
"""Sets the robot's configuration so the gripper grasps object at
one of its potential grasp locations. Might change self.active_limb
to the appropriate limb.
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
# the gripper center transform is given in the camera frame
# so use the camera link and give the fix point as the gripper center
for link, gripper_center, side in [ (self.left_gripper_link, left_gripper_center_xform[1], 'left'), \
(self.right_gripper_link, right_gripper_center_xform[1], 'right') ]:
# check all possible grasp points
for grasp_xform in self.knowledge.grasp_xforms(object):
# compute the grasp point using the gripper center offset
grasp_point = se3.apply(grasp_xform, vectorops.mul(gripper_center, -1))
#grasp_point = vectorops.sub(grasp_xform[1], gripper_center)
# move the gripper center to grasp point aligned with the grasp frame
# note use of the camera link frame since the grasp offset is specified in that frame
goal = ik.objective(link, R=grasp_xform[0], t=grasp_point)
# reset the robot to the starting config
goal.robot.setConfig(self.config)
# try with 50 attempts
# randomize after each failed attempt
for attempts in range(50):
# solve the inverse kinematics with 1 mm accuracy
if ik.solve(goal, tol=0.001):
self.active_limb = side
self.config = goal.robot.getConfig()
return True
else:
# randomize this limb
randomize_links(goal.robot, arm_link_names[side])
return False
def spawn_objects_from_ground_truth(world):
"""For all ground_truth_items, spawns RigidObjects in the world
according to their sizes / mass properties"""
print "Initializing world objects"
for item in ground_truth_items:
obj = world.makeRigidObject(item.info.name)
bmin,bmax = item.info.bmin,item.info.bmax
center = vectorops.div(vectorops.add(bmin,bmax),2.0)
m = obj.getMass()
m.setMass(item.info.mass)
m.setCom([0,0,0])
m.setInertia(vectorops.mul([bmax[0]-bmin[0],bmax[1]-bmin[1],bmax[2]-bmin[2]],item.info.mass/12.0))
obj.setMass(m)
c = obj.getContactParameters()
c.kFriction = 0.6
c.kRestitution = 0.1;
c.kStiffness = 100000
c.kDamping = 100000
obj.setContactParameters(c)
simgeometry = obj.geometry()
load_item_geometry(item,simgeometry)
obj.setTransform(item.xform[0],item.xform[1])
return
def onMessage(self, msg):
global deviceState, defaultDeviceState
#print "Getting haptic message"
#print msg
if msg['type'] != 'MultiHapticState':
print "Strange message type", msg['type']
return
if len(deviceState) == 0:
print "Adding", len(
msg['devices']) - len(deviceState), "haptic devices"
while len(deviceState) < len(msg['devices']):
deviceState.append(defaultDeviceState.copy())
if len(msg['devices']) != len(deviceState):
print "Incorrect number of devices in message:", len(
msg['devices'])
return
#change overall state depending on button 2
if 'events' in msg:
for e in msg['events']:
#print e['type']
if e['type'] == 'b2_down':
dstate = deviceState[e['device']]
toggleMode(dstate)
print 'Changing device', e['device'], 'to state', dstate[
'mode']
for i in range(len(deviceState)):
dmsg = msg['devices'][i]
dstate = deviceState[i]
if dmsg['button1'] == 1:
#drag widget if button 1 is down
oldButtonDown = dstate['buttonDown']
dstate['buttonDown'] = True
#moving
if dstate['mode'] == 'normal':
if not oldButtonDown:
dstate['moveStartDeviceTransform'] = (so3.from_moment(
dmsg['rotationMoment']), dmsg['position'])
if self.widgetGetters == None:
dstate['moveStartWidgetTransform'] = dstate[
'widgetTransform']
else:
Twidget = self.widgetGetters[i]()
if Twidget != None:
dstate['moveStartWidgetTransform'] = Twidget
else:
dstate['moveStartWidgetTransform'] = dstate[
'widgetTransform']
#================================================================================================
# #perform transformation of widget
# deviceTransform = (so3.from_moment(dmsg['rotationMoment']),dmsg['position'])
# #compute relative motion
# Tdev = se3.mul(deviceToWidgetTransform,deviceTransform)
# TdevStart = se3.mul(deviceToWidgetTransform,dstate['moveStartDeviceTransform'])
# relDeviceTransform = (so3.mul(Tdev[0],so3.inv(TdevStart[0])),vectorops.sub(Tdev[1],TdevStart[1]))
# #scale relative motion
# Rrel,trel = relDeviceTransform
# wrel = so3.moment(Rrel)
# wrel = vectorops.mul(wrel,dstate['rotationScale'])
# trel = vectorops.mul(trel,dstate['positionScale'])
# #print "Moving",trel,"rotating",wrel
# relDeviceTransform = (so3.from_moment(wrel),trel)
# #perform transform
# dstate['widgetTransform'] = se3.mul(dstate['moveStartWidgetTransform'],relDeviceTransform)
#================================================================================================
#- perform transformation of widget
deviceTransform = (so3.from_moment(dmsg['rotationMoment']),
dmsg['position'])
# delta_device = vectorops.sub(deviceTransform[1],dstate['moveStartDeviceTransform'][1])
# print "haptic moves: ", delta_device
delta_device_R_matrix = so3.mul(
deviceTransform[0],
so3.inv(dstate['moveStartDeviceTransform'][0]))
# delta_device_R = so3.moment(delta_device_R_matrix)
# norm_delta_R = vectorops.norm(delta_device_R)
# if norm_delta_R != 0:
# vec_delta_R = vectorops.div(delta_device_R, norm_delta_R)
# else:
# vec_delta_R = [0,0,0]
#
# print "haptic rotate: norm = ", norm_delta_R, ", vec = ", vec_delta_R
#- compute relative motion
Tdev = se3.mul(deviceToBaxterBaseTransform,
deviceTransform)
TdevStart = se3.mul(deviceToBaxterBaseTransform,
dstate['moveStartDeviceTransform'])
# delta_widget = vectorops.sub(Tdev[1],TdevStart[1])
# print "Baxter moves: ", delta_widget
# delta_widget_R_matrix = so3.mul(Tdev[0],so3.inv(TdevStart[0]))
# delta_widget_R = so3.moment(delta_widget_R_matrix)
# norm_widget_R = vectorops.norm(delta_widget_R)
# if norm_widget_R != 0:
# vec_widget_R = vectorops.div(delta_widget_R, norm_widget_R)
# else:
# vec_widget_R = [0,0,0]
# print "Baxter rotate: norm = ", norm_widget_R, ", vec = ", vec_widget_R
relDeviceTransform = (so3.mul(Tdev[0],
so3.inv(TdevStart[0])),
vectorops.sub(Tdev[1], TdevStart[1]))
#- scale relative motion
Rrel, trel = relDeviceTransform
wrel = so3.moment(Rrel)
wrel = vectorops.mul(wrel, dstate['rotationScale'])
trel = vectorops.mul(trel, dstate['positionScale'])
#- print "Moving",trel,"rotating",wrel
relDeviceTransform = (so3.from_moment(wrel), trel)
#- perform transform
# dstate['widgetTransform'] = se3.mul(dstate['moveStartWidgetTransform'],relDeviceTransform)
dstate['widgetTransform'] = (
so3.mul(dstate['moveStartWidgetTransform'][0],
relDeviceTransform[0]),
vectorops.add(dstate['moveStartWidgetTransform'][1],
relDeviceTransform[1]))
#================================================================================================
elif dstate['mode'] == 'scaling':
if not oldButtonDown:
dstate['moveStartDeviceTransform'] = (so3.from_moment(
dmsg['rotationMoment']), dmsg['position'])
#perform scaling
deviceTransform = (so3.from_moment(dmsg['rotationMoment']),
dmsg['position'])
oldDeviceTransform = dstate['moveStartDeviceTransform']
znew = deviceTransform[1][1]
zold = oldDeviceTransform[1][1]
posscalerange = [1e-2, 20]
rotscalerange = [0.5, 2]
newposscale = min(
max(
dstate['positionScale'] * math.exp(
(znew - zold) * 10), posscalerange[0]),
posscalerange[1])
newrotscale = min(
max(
dstate['rotationScale'] * math.exp(
(znew - zold) * 4), rotscalerange[0]),
rotscalerange[1])
if any(newposscale == v for v in posscalerange) or any(
newrotscale == v for v in rotscalerange):
pass #dont change the scale
else:
dstate['positionScale'] = newposscale
dstate['rotationScale'] = newrotscale
print "Setting position scale to", dstate[
'positionScale'], "rotation scale to", dstate[
'rotationScale']
#update start device transform
dstate['moveStartDeviceTransform'] = deviceTransform
elif dstate['mode'] == 'gripper':
print "Gripper mode not available"
dstate['mode'] = 'normal'
dstate['buttonDown'] = False
else:
dstate['buttonDown'] = False
if self.viewer: self.viewer.update(deviceState)
else: print "No viewer..."
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 autoInertia(self):
bmin,bmax = self.bmin,self.bmax
self.inertia = vectorops.mul([bmax[0]-bmin[0],bmax[1]-bmin[1],bmax[2]-bmin[2]],self.mass/12.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)
def power_grasp_medial_axis(pc,bin,object,knowledgeBase):
"""Returns a power grasp for the Reflex gripper to grasp the medial axis
of the point cloud given in the Baxter's frame (i.e., x is forward, z
is up).
Input:
- pc: a klampt.PointCloud object
- bin: a bin name
- object: an object name
- knowledgeBase: an apc.KnowledgeBase object
Returns a pair (g,s):
- g: an apc.ItemGrasp member defining the grasp. Its xform member is
given in the Baxter's local frame.
- s: a confidence score, with 0 = unconfident, 1 = confident
"""
global gripper,depthmax,powerwidthmax
points = []
for i in xrange(0,len(pc.vertices),3):
points.append([pc.vertices[i],pc.vertices[i+1],pc.vertices[i+2]])
mean = [0,0,0]
for p in points:
mean = vectorops.add(mean,p)
mean = vectorops.mul(mean,1.0/len(points))
bmin,bmax = points[0][:],points[0][:]
for p in points:
if p[0] < bmin[0]: bmin[0]=p[0]
elif p[0] > bmax[0]: bmax[0]=p[0]
if p[1] < bmin[1]: bmin[1]=p[1]
elif p[1] > bmax[1]: bmax[1]=p[1]
if p[2] < bmin[2]: bmin[2]=p[2]
elif p[2] > bmax[2]: bmax[2]=p[2]
median = vectorops.mul(vectorops.add(bmin,bmax),0.5)
width = bmax[1]-bmin[1]
depth = bmax[0]-bmin[0]
height = bmax[2]-bmin[2]
#determine an adequate center
graspcenter = median[:]
if depth*0.5 > depthmax - center_to_finger:
graspcenter[0] = bmin[0] + depthmax - center_to_finger
graspfile = os.path.join("../resources",gripper,object,'default_power_grasp.json')
try:
f = open(graspfile,'r')
contents = ''.join(f.readlines())
g = apc.ItemGrasp()
g.readFromJson(contents)
#translate to center of point cloud
g.grasp_xform = (g.grasp_xform[0],vectorops.add(g.grasp_xform[1],graspcenter))
return (g,1)
except IOError:
print "No default grasp defined for object",object,"trying default"
graspfile = os.path.join("../resources",gripper,'default_power_grasp.json')
try:
f = open(graspfile,'r')
contents = ''.join(f.readlines())
g = apc.ItemGrasp()
g.readFromJson(contents)
#translate to center of point cloud
g.grasp_xform = (g.grasp_xform[0],vectorops.add(g.grasp_xform[1],graspcenter))
#adjust grasp to size of object
ratio = width/powerwidthmax
if ratio > 1.0:
g.gripper_close_command[0] = g.gripper_close_command[1] = g.gripper_close_command[2] = 0.7
g.gripper_open_command[0] = g.gripper_open_command[1] = g.gripper_open_command[2] = g.gripper_close_command[2] = 1
return (g,0.1)
else:
#simple approximation: angular interpolation
angle = math.asin(ratio)
g.gripper_close_command[0] = g.gripper_close_command[1] = g.gripper_close_command[2] = 0.7*angle/(math.pi*0.5)
g.gripper_open_command[0] = g.gripper_open_command[1] = g.gripper_open_command[2] = g.gripper_open_command[2] = g.gripper_close_command[0]+0.2
return (g,0.5)
except IOError:
print "Default power grasp file",graspfile,"not found"
return (None,0)
