def drawContactForces(self):
contacted=False;
for i in range(self.simworld.numIDs()):
for j in range(i+1, self.simworld.numIDs()):
if(self.sim.hadContact(i,j)):
if(not contacted):
# print "Touching bodies:\n"
contacted=True
f = self.sim.meanContactForce(i,j)
contacts = self.sim.getContacts(i,j)
# print self.simworld.getName(i),self.simworld.getName(j), len(contacts)
x=[0,0,0]
n=[0,0,0]
k=0
scale = 5
if len(contacts) != 0:
for numContact in range(len(contacts)):
x = vectorops.add(x,contacts[numContact][0:3])
n = vectorops.add(n,contacts[numContact][3:6])
k += contacts[numContact][6]
x = vectorops.div(x,len(contacts))
n = vectorops.div(n,len(contacts))
k = k/(len(contacts)*scale)
draw_vector(x,n,k)
def angle(a, b):
anorm = vectorops.norm(a)
bnorm = vectorops.norm(b)
if anorm < 1e-6 or bnorm < 1e-6: return 0
dp = vectorops.dot(vectorops.div(a, anorm), vectorops.div(b, bnorm))
dp = max(min(dp, 1), -1)
return math.acos(dp)
def saveStep(self, extra=[]):
sim = self.sim
world = sim.world
sim.updateWorld()
values = []
values.append(sim.getTime())
for i in xrange(world.numRobots()):
robot = world.robot(i)
values += robot.getCom()
values += robot.getConfig()
values += robot.getVelocity()
values += sim.getActualTorques(i)
j = 0
while True:
s = self.sim.controller(i).sensor(j)
if s is None or len(s.name()) == 0:
break
meas = s.getMeasurements()
values += meas
j += 1
for i in xrange(world.numRigidObjects()):
obj = world.rigidObject(i)
T = obj.getTransform()
values += se3.apply(T, obj.getMass().getCom())
values += T[1]
values += so3.moment(T[0])
values += sim.body(obj).getVelocity()[1]
values += sim.body(obj).getVelocity()[0]
if self.f_contact:
for i, id in enumerate(self.colliding):
for j in range(i + 1, len(self.colliding)):
id2 = self.colliding[j]
if sim.hadContact(id, id2):
clist = sim.getContacts(id, id2)
f = sim.contactForce(id, id2)
m = sim.contactTorque(id, id2)
pavg = [0.0] * 3
navg = [0.0] * 3
for c in clist:
pavg = vectorops.add(pavg, c[0:3])
navg = vectorops.add(navg, c[3:6])
if len(clist) > 0:
pavg = vectorops.div(pavg, len(clist))
navg = vectorops.div(navg, len(clist))
body1 = world.getName(id)
body2 = world.getName(id2)
cvalues = [sim.getTime(), body1, body2, len(clist)]
cvalues += pavg
cvalues += navg
cvalues += f
cvalues += m
self.f_contact.write(','.join(str(v) for v in cvalues))
self.f_contact.write('\n')
if extra:
values += extra
if not (self.f is None):
self.f.write(','.join([str(v) for v in values]))
self.f.write('\n')
def saveStep(self,extra=[]):
sim = self.sim
world = sim.world
sim.updateWorld()
values = []
values.append(sim.getTime())
for i in xrange(world.numRobots()):
robot = world.robot(i)
values += robot.getCom()
values += robot.getConfig()
values += sim.getActualTorques(i)
"""
j = 0
while True:
s = self.sim.controller(i).getSensor(j)
if len(s.name())==0:
break
meas = s.measurements()
values += meas
j += 1
"""
for i in xrange(world.numRigidObjects()):
obj = world.rigidObject(i)
T = obj.getTransform()
values += se3.apply(T,obj.getMass().getCom())
values += T[1]
values += so3.moment(T)
values += sim.body(obj).getVelocity()[1]
values += sim.body(obj).getVelocity()[0]
if self.f_contact:
for i,id in enumerate(self.colliding):
for j in range(i+1,len(self.colliding)):
id2 = self.colliding[j]
if sim.hadContact(id,id2):
clist = sim.getContacts(id,id2);
f = sim.contactForce(id,id2)
m = sim.contactTorque(id,id2)
pavg = [0.0]*3
navg = [0.0]*3
for c in clist:
pavg = vectorops.add(pavg,c[0:3])
navg = vectorops.add(navg,c[3:6])
if len(clist) > 0:
pavg = vectorops.div(pavg,len(clist))
navg = vectorops.div(navg,len(clist))
body1 = world.getName(id)
body2 = world.getName(id2)
values = [sim.getTime(),body1,body2,len(clist)]
values += pavg
values += navg
values += f
values += m
self.f_contact.write(','.join(str(v) for v in values))
self.f_contact.write('\n')
if extra:
values += extra
self.f.write(','.join([str(v) for v in values]))
self.f.write('\n')
def sendPath(path,maxSmoothIters =0, INCREMENTAL=False, limb = None, readConstants=False, internalSpeed=SPEED):
# interpolate path linearly between two endpoints
if path == None:
print "sending empty path"
return False
for smoothIter in range(maxSmoothIters):
# path = path
smoothePath = [0]*(len(path)*2-1)
for i in range(len(path)-1):
smoothePath[i*2] = path[i]
smoothePath[i*2 +1] = vectorops.div(vectorops.add(path[i],path[i+1]), 2)
smoothePath[-1] = path[-1]
path = smoothePath
while i <endIndex-1:
# print i, endIndex
q = path[i]
qNext = path[i+1]
dt = vectorops.distance(q,qNext)
# smooth trajectory by interpolating between two consecutive configurations
# if the distance between the two is big
# Note: might want to make dt bigger for arm movements (only 7 configurations vs 52)
if dt>0.1:
qInterp = vectorops.div(vectorops.add(q, qNext), 2)
path.insert(i+1, qInterp)
endIndex +=1
continue
else:
i += 1
waitForMove()
if counter%internalSpeed == 0 or INCREMENTAL:
if limb == 'left':
#CONTROLLER.appendMilestoneLeft(q)
pass
elif limb == 'right':
#CONTROLLER.appendMilestoneRight(q)
pass
else:
#print 'milestone #', i, q
#CONTROLLER.appendMilestone(q)
pass
counter +=1
if limb == 'left':
#CONTROLLER.appendMilestoneLeft(path[-1])
pass
elif limb == 'right':
#CONTROLLER.appendMilestoneRight(path[-1])
pass
else:
pass
def point_fit_xform_3d(apts,bpts):
"""Finds a 3D rigid transform that maps the list of points apts to the
list of points bpts. Return value is a klampt.se3 element that
minimizes the sum of squared errors ||T*ai-bi||^2.
"""
assert len(apts)==len(bpts)
ca = vectorops.div(reduce(apts,vectorops.add),len(apts))
cb = vectorops.div(reduce(bpts,vectorops.add),len(bpts))
arel = [vectorops.sub(a,ca) for a in apts]
brel = [vectorops.sub(b,cb) for b in bpts]
R = point_fit_rotation_3d(arel,brel)
#R minimizes sum_{i=1,...,n} ||R(ai-ca) - (bi-cb)||^2
t = so3.sub(cb,so3.apply(R,ca))
return (R,t)
def point_fit_xform_3d(apts,bpts):
"""Finds a 3D rigid transform that maps the list of points apts to the
list of points bpts. Return value is a klampt.se3 element that
minimizes the sum of squared errors ||T*ai-bi||^2.
"""
assert len(apts)==len(bpts)
ca = vectorops.div(reduce(apts,vectorops.add),len(apts))
cb = vectorops.div(reduce(bpts,vectorops.add),len(bpts))
arel = [vectorops.sub(a,ca) for a in apts]
brel = [vectorops.sub(b,cb) for b in bpts]
R = point_fit_rotation_3d(arel,brel)
#R minimizes sum_{i=1,...,n} ||R(ai-ca) - (bi-cb)||^2
t = so3.sub(cb,so3.apply(R,ca))
return (R,t)
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 output_and_advance(self, **inputs):
try:
q = inputs['q']
dq = inputs['dq']
u = vectorops.div(vectorops.sub(inputs['qcmd'], q), inputs['dt'])
except KeyError:
print "Warning, cannot debug motion model, dq or dqcmd not in input"
return None
if self.dqpredlast != None:
if self.activeDofs != None:
dq = dq[:]
for i in [
i for i in range(len(q)) if i not in self.activeDofs
]:
dq[i] = self.dqpredlast[i]
#compare motion model to dq
print "Motion model error:", np.linalg.norm(self.dqpredlast -
np.array(dq))
(v, i) = max(
zip(
np.abs(self.dqpredlast - np.array(dq)).tolist(),
range(len(dq))))
print " Max error:", v, "at", i,
if self.robot != None: print self.robot.getLink(i).getName()
else: print
print " Command:", self.ulast[i], "Predicted:", self.dqpredlast[
i], "Actual:", dq[i]
print " pred:", self.Alast[i, i], "*u +", self.blast[i]
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 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 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 update_world_with_knowledge(self):
'''Create objects in the planning for those added to the
knowledge base.'''
# look at all the bins
for (name, contents) in self.knowledge.bin_contents.items():
# only care above perceived for full bins
if contents:
# look at all the items in the bin
for item in contents:
# check which ones are missing planning objects
if item.klampt_index is None:
# create an object for it
# code borrowed from spawn_objects_from_ground_truth
obj = self.world.makeRigidObject(item.info.name)
bmin,bmax = item.info.bmin,item.info.bmax
center = vectorops.div(vectorops.add(bmin,bmax),2.0)
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)
obj.setTransform(item.xform[0],item.xform[1])
# note this item's corresponding object
item.klampt_index = obj.getID()
# update the collider
self.collider = WorldCollider(self.world)
print 'spawned planning model', obj.getName()
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 output_and_advance(self,**inputs):
try:
dt = inputs["dt"]
q = inputs["q"]
except KeyError:
raise ValueError("Input needs to have configuration 'q' and timestep 'dt'")
if len(q)==0: return None
if self.qlast==None:
dq = [0]*len(q)
else:
if self.robot==None:
dq = vectorops.div(self.robot.sub(q,self.qlast),dt)
else:
assert(len(self.qlast)==len(q))
dq = vectorops.div(self.robot.interpolate_deriv(self.qlast,q),dt)
self.qlast = q
return {'dq':dq}
def interpolate_rotation(R1, R2, u):
"""Interpolate linearly between the two rotations R1 and R2.
TODO: the current implementation doesn't work properly. Why? """
m1 = so3.moment(R1)
m2 = so3.moment(R2)
mu = interpolate_linear(m1, m2, u)
angle = vectorops.norm(mu)
axis = vectorops.div(mu, angle)
return so3.rotation(axis, angle)
def interpolate_rotation(R1,R2,u):
"""Interpolate linearly between the two rotations R1 and R2.
TODO: the current implementation doesn't work properly. Why? """
m1 = so3.moment(R1)
m2 = so3.moment(R2)
mu = interpolate_linear(m1,m2,u)
angle = vectorops.norm(mu)
axis = vectorops.div(mu,angle)
return so3.rotation(axis,angle)
def output_and_advance(self, **inputs):
try:
dt = inputs["dt"]
q = inputs["q"]
except KeyError:
raise ValueError(
"Input needs to have configuration 'q' and timestep 'dt'")
if len(q) == 0: return None
if self.qlast == None:
dq = [0] * len(q)
else:
if self.robot == None:
dq = vectorops.div(self.robot.sub(q, self.qlast), dt)
else:
assert (len(self.qlast) == len(q))
dq = vectorops.div(self.robot.interpolate_deriv(self.qlast, q),
dt)
self.qlast = q
return {'dq': dq}
def getSensedVelocity(self, q, dq, dt):
"""Gets task velocity from sensed configuration q.
Default implementation uses finite differencing. Other
implementations may use jacobian.
"""
#uncomment this to get a jacobian based technique
#return np.dot(self.getJacobian(q),dq)
if self.qLast == None:
return None
else:
xlast = self.getSensedValue(self.qLast)
xcur = self.getSensedValue(q)
return vectorops.div(self.taskDifference(xcur, xlast), dt)
def getSensedVelocity(self, q, dq, dt): """Gets task velocity from sensed configuration q. Default implementation uses finite differencing. Other implementations may use jacobian. """ #uncomment this to get a jacobian based technique #return np.dot(self.getJacobian(q),dq) if self.qLast==None: return None else: xlast = self.getSensedValue(self.qLast) xcur = self.getSensedValue(q) return vectorops.div(self.taskDifference(xcur,xlast),dt)
def setDesiredVelocitiesFromDifference(self,qdes0,qdes1,dt,tasks=None): """Sets all the tasks' desired velocities from a given pair of configurations separated by dt (e.g., to follow a reference trajectory). If the 'tasks' variable is provided, it should be a list of tasks for which the desired values should be set. """ if tasks == None: tasks = self.taskList for t in tasks: xdes0 = t.getSensedValue(qdes0) xdes1 = t.getSensedValue(qdes1) dx = vectorops.div(t.taskDifference(xdes1,xdes0),dt) t.setDesiredVelocity(dx)
def setDesiredVelocitiesFromDifference(self, qdes0, qdes1, dt, tasks=None):
"""Sets all the tasks' desired velocities from a given pair
of configurations separated by dt (e.g., to follow a reference
trajectory).
If the 'tasks' variable is provided, it should be a list of
tasks for which the desired values should be set.
"""
if tasks == None:
tasks = self.taskList
for t in tasks:
xdes0 = t.getSensedValue(qdes0)
xdes1 = t.getSensedValue(qdes1)
dx = vectorops.div(t.taskDifference(xdes1, xdes0), dt)
t.setDesiredVelocity(dx)
def advance(self, **inputs):
try:
qcmd = inputs['qcmd']
q = inputs['q']
dt = inputs['dt']
except KeyError:
return
if len(q) == 0: return
print len(qcmd), len(q)
u = vectorops.sub(qcmd, q)
u = vectorops.div(u, dt)
print "u estimate:", u
print "u estimate rarm:", u[26:33]
try:
q = inputs[self.xnames[0]]
dq = inputs[self.xnames[1]]
except KeyError as e:
print "Warning, inputs do not contain x key", e
return
"""
try:
u = sum((inputs[uname] for uname in self.unames),[])
except KeyError as e:
print "Warning, inputs do not contain u key",e
#u = [0]*(len(x)/2)
try:
u = vectorops.sub(inputs['qcmd'],inputs['q'])
u = vectorops.div(u,inputs['dt'])
except KeyError as e:
print "Warning, inputs do not contain qcmd key either",e
u = [0]*(len(x)/2)
"""
#self.xlast = x
#return
#do system ID
if self.qlast != None:
self.estimator.add(self.qlast, self.dqlast, u, q, dq)
pass
#update last state
self.qlast = q
self.dqlast = dq
return
def advance(self,**inputs):
try:
qcmd = inputs['qcmd']
q = inputs['q']
dt = inputs['dt']
except KeyError:
return
if len(q)==0: return
print len(qcmd),len(q)
u = vectorops.sub(qcmd,q)
u = vectorops.div(u,dt)
print "u estimate:",u
print "u estimate rarm:",u[26:33]
try:
q = inputs[self.xnames[0]]
dq = inputs[self.xnames[1]]
except KeyError as e:
print "Warning, inputs do not contain x key",e
return
"""
try:
u = sum((inputs[uname] for uname in self.unames),[])
except KeyError as e:
print "Warning, inputs do not contain u key",e
#u = [0]*(len(x)/2)
try:
u = vectorops.sub(inputs['qcmd'],inputs['q'])
u = vectorops.div(u,inputs['dt'])
except KeyError as e:
print "Warning, inputs do not contain qcmd key either",e
u = [0]*(len(x)/2)
"""
#self.xlast = x
#return
#do system ID
if self.qlast != None:
self.estimator.add(self.qlast,self.dqlast,u,q,dq)
pass
#update last state
self.qlast = q
self.dqlast = dq
return
def output_and_advance(self,**inputs):
try:
q = inputs['q']
dq = inputs['dq']
u = vectorops.div(vectorops.sub(inputs['qcmd'],q),inputs['dt'])
except KeyError:
print "Warning, cannot debug motion model, dq or dqcmd not in input"
return None
if self.dqpredlast != None:
if self.activeDofs != None:
dq = dq[:]
for i in [i for i in range(len(q)) if i not in self.activeDofs]:
dq[i] = self.dqpredlast[i]
#compare motion model to dq
print "Motion model error:",np.linalg.norm(self.dqpredlast - np.array(dq))
(v,i) = max(zip(np.abs(self.dqpredlast - np.array(dq)).tolist(),range(len(dq))))
print " Max error:",v,"at",i,
if self.robot!=None: print self.robot.getLink(i).getName()
else: print
print " Command:",self.ulast[i],"Predicted:",self.dqpredlast[i],"Actual:",dq[i]
print " pred:",self.Alast[i,i],"*u +",self.blast[i]
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 generate_trajectory(qi, qf):
i = 0
endIndex = 2
path = [0.0, 0.0]
path[0] = qi
path[1] = qf
print qi, qf
while i < endIndex-1:
# print i, endIndex
q = path[i]
qNext = path[i+1]
dt = vectorops.distance(q,qNext)
# smooth trajectory by interpolating between two consecutive configurations
# if the distance between the two is big
if dt>0.1:
qInterp = vectorops.div(vectorops.add(q, qNext), 2)
path.insert(i+1, qInterp)
endIndex +=1
continue
else:
i += 1
print len(path)
return path
elif cmd == 'find':
print master._find_object()
elif cmd == 'grasp':
print master._grasp_object()
elif cmd == 'retrieve':
print master._retrieve_object()
elif cmd == 'move_cartesian' and len(args) == 4:
task = None
limb = args[0]
amount = [float(args[1]),float(args[2]),float(args[3])]
maxspeed = 0.01
command = {'limb':limb,'velocity':vectorops.div(amount,vectorops.norm(amount)/maxspeed),'duration':vectorops.norm(amount)/maxspeed}
print "Trying cartesian_drive low level command"
task = master.manager.control.execute([
( 'cartesian_drive', command, 0, 0),
])
while task and not task.done:
sleep(0.1)
elif cmd == 'gripper' and len(args) == 1:
task = None
if args[0] == 'parallel':
task = master.manager.control.execute([
( 'left_gripper', [0.5, 0.5, 0.5, 1], 0, 0),
( 'right_gripper', [0.5, 0.5, 0.5, 1], 0, 0),
def sendPath(path,maxSmoothIters =0, INCREMENTAL=False, limb = None, readConstants=False, internalSpeed=SPEED):
# interpolate path linearly between two endpoints
if path == None:
print "sending empty path"
return False
# n = self.robot.numLinks()
# for i in range(len(path)):
# # if we specify a limb, and the path only has seven numbers (DOF for each arm, we shouldn't append 0's)
# if readConstants and limb is not None:
# # if we're reading a path from the milestones
# pass
# else:
# #print path
# if len(path[i])<n:
# path[i] += [0.0]*(n-len(path[i]))
# #pass
for smoothIter in range(maxSmoothIters):
# path = path
smoothePath = [0]*(len(path)*2-1)
for i in range(len(path)-1):
smoothePath[i*2] = path[i]
smoothePath[i*2 +1] = vectorops.div(vectorops.add(path[i],path[i+1]), 2)
smoothePath[-1] = path[-1]
path = smoothePath
i = 0
endIndex = len(path)
# if endIndex==1:
# q=path[0]
# path[0]=self.robot.getConfig()
# path.append(q)
counter = 0
#path.append(path[-1])
#endIndex = len(path)
#print 'myPath = ', path
speed = 1
while i+1 <endIndex:
# print i, endIndex
q = path[i]
qNext = path[i+1]
dt = vectorops.distance(q,qNext)
dt = dt / speed
i += 1
waitForMove()
# if INCREMENTAL:
# time.sleep(.1)
if limb == 'left':
CONTROLLER.appendMilestoneLeft(q,dt)
#pass
elif limb == 'right':
CONTROLLER.appendMilestoneRight(q,dt)
#pass
else:
#print 'milestone #', i, q
#CONTROLLER.appendMilestone(q)
CONTROLLER.appendLinear(q,dt)
#pass
"""
def loop(self):
try:
while True:
print OKBLUE + "Bin " + str(self.current_bin) + ": " + self.state + END_COLOR
if self.state == "VISUAL_DEBUG":
self.object_com = [1.0839953170961105, -0.25145094946424207, 1.1241831909823194]
# self.set_model_right_arm( [0.1868786927065213, -1.567604604679142, 0.6922768776941961, 1.5862815343628953, -0.005567750307534711, -0.017979599494945674, 0.0035268645585939083])
self.load_real_robot_state()
else:
self.load_real_robot_state()
self.Tcamera = se3.mul(
self.robotModel.link("right_lower_forearm").getTransform(), self.calibratedCameraXform
)
self.Tvacuum = se3.mul(self.robotModel.link("right_wrist").getTransform(), VACUUM_POINT_XFORM)
self.vacuumPc = Geometry3D()
self.vacuumPc.loadFile(VACUUM_PCD_FILE)
temp_xform = self.robotModel.link("right_wrist").getTransform()
self.vacuumPc.transform(self.Tvacuum[0], self.Tvacuum[1])
if self.state == "DEBUG_COLLISION_CHECKER":
temp_planner = planning.LimbPlanner(self.world, self.vacuumPc)
print "Is this configuration collision free?"
print temp_planner.check_collision_free("right")
sys.stdout.flush()
elif self.state == "FAKE_PATH_PLANNING":
self.object_com = [1.114, -0.077, 1.412]
self.right_arm_ik(self.object_com)
self.Tcamera = se3.mul(
self.robotModel.link("right_lower_forearm").getTransform(), self.calibratedCameraXform
)
self.Tvacuum = se3.mul(self.robotModel.link("right_wrist").getTransform(), VACUUM_POINT_XFORM)
time.sleep(2342340)
elif self.state == "START":
self.state = "MOVE_TO_SCAN_BIN"
elif self.state == "MOVE_TO_SCAN_BIN":
for milestone in eval("Q_BEFORE_SCAN_" + self.current_bin):
print "Moving to " + str(milestone)
motion.robot.right_mq.appendLinear(MOVE_TIME, planning.cleanJointConfig(milestone))
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig(eval("Q_SCAN_BIN_" + self.current_bin))
)
self.state = "MOVING_TO_SCAN_BIN"
elif self.state == "MOVING_TO_SCAN_BIN":
if not motion.robot.right_mq.moving():
self.wait_start_time = time.time()
self.state = "WAITING_TO_SCAN_BIN"
elif self.state == "WAITING_TO_SCAN_BIN":
if time.time() - self.wait_start_time > SCAN_WAIT_TIME:
if REAL_PERCEPTION:
self.state = "SCANNING_BIN"
else:
self.state = "FAKE_SCANNING_BIN"
elif self.state == "SCANNING_BIN":
print "Waiting for message from camera"
cloud = rospy.wait_for_message(ROS_DEPTH_TOPIC, PointCloud2)
if perception.isCloudValid(cloud):
cloud = perception.convertPc2ToNp(cloud)
np_cloud = cloud[::STEP]
self.points1 = []
for point in np_cloud:
transformed = se3.apply(self.Tcamera, point)
self.points1.append(transformed)
np_cloud = perception.subtractShelf(np_cloud, self.current_bin)
self.points2 = []
for point in np_cloud:
transformed = se3.apply(self.Tcamera, point)
self.points2.append(transformed)
self.object_com = se3.apply(self.Tcamera, perception.com(np_cloud))
if PRINT_BLOBS:
print np_cloud
sio.savemat(CLOUD_MAT_PATH, {"cloud": np_cloud})
fo = open(CHENYU_GO_PATH, "w")
fo.write("chenyugo")
fo.close()
self.object_com = se3.apply(self.Tcamera, perception.com(np_cloud))
if CALIBRATE_CAMERA:
self.state = "CALIBRATE_CAMERA"
elif SEGMENT:
self.state = "WAITING_FOR_SEGMENTATION"
else:
self.state = "MOVE_TO_GRASP_OBJECT"
else:
print FAIL_COLOR + "Got an invalid cloud, trying again" + END_COLOR
elif self.state == "FAKE_SCANNING_BIN":
self.object_com = [1.1114839836097854, -0.6087936130127559, 0.9899267043340634]
if DRY_RUN:
raw_input("Hit enter to continue: ")
self.state = "MOVE_TO_GRASP_OBJECT"
elif self.state == "CALIBRATE_CAMERA":
self.calibrateCamera()
self.state = "SCANNING_BIN"
elif self.state == "WAITING_FOR_SEGMENTATION":
if os.path.isfile(CHENYU_DONE_PATH):
os.remove(CHENYU_DONE_PATH)
self.state = "POSTPROCESS_SEGMENTATION"
elif self.state == "POSTPROCESS_SEGMENTATION":
object_blobs = []
time.sleep(5)
for i in range(1, 20):
seg_file_path = MAT_PATH + "seg" + str(i) + ".mat"
print seg_file_path
if os.path.isfile(seg_file_path):
print seg_file_path + " exists"
mat_contents = sio.loadmat(seg_file_path)
r = mat_contents["r"]
r = r[r[:, 1] != 0, :]
object_blobs.append(r)
os.remove(seg_file_path)
if PRINT_BLOBS:
print "============="
print "object blobs"
print object_blobs
print "============="
if len(object_blobs) == 0:
self.state = "BIN_DONE"
else:
object_list = [
ITEM_NUMBERS[item_str] for item_str in self.bin_state[self.current_bin]["contents"]
]
target = ITEM_NUMBERS[self.bin_state[self.current_bin]["target"]]
histogram_dict = perception.loadHistogram(object_list)
cloud_label = {} # key is the label of object, value is cloud points
label_score = {} # key is the label, value is the current score for the object
for object_cloud in object_blobs:
if PRINT_BLOBS:
print "====================="
print "object_cloud"
print object_cloud
print "====================="
object_cloud = perception.resample(cloud, object_cloud, 3)
label, score = perception.objectMatch(object_cloud, histogram_dict)
if label in cloud_label:
if label_score[label] < score:
label_score[label] = score
cloud_label[label] = object_cloud
else:
cloud_label[label] = object_cloud
label_score[label] = score
if PRINT_LABELS_AND_SCORES:
print cloud_label
print "============="
print label_score
if target in cloud_label:
self.object_com = se3.apply(self.Tcamera, perception.com(cloud_label[target]))
self.points1 = []
for point in cloud_label[target]:
transformed = se3.apply(self.Tcamera, point)
self.points1.append(transformed)
self.points2 = []
else:
cloud_score = {}
histogram_dict = perception.loadHistogram([target])
for object_cloud in object_blobs:
object_cloud = perception.resample(cloud, object_cloud, 3)
label, score = perception.objectMatch(object_cloud, histogram_dict)
cloud_score[score] = object_cloud
sorted_cloud = sorted(cloud_score.items(), key=operator.itemgetter(0), reverse=True)
score = sorted_cloud[0][0]
com = perception.com(sorted_cloud[0][1])
self.points1 = []
self.points2 = []
for point in sorted_cloud[0][1]:
transformed = se3.apply(self.Tcamera, point)
self.points1.append(transformed)
self.object_com = se3.apply(self.Tcamera, com)
self.state = "MOVE_TO_GRASP_OBJECT"
elif self.state == "MOVE_TO_GRASP_OBJECT":
for milestone in eval("Q_AFTER_SCAN_" + self.current_bin):
print "Moving to " + str(milestone)
motion.robot.right_mq.appendLinear(MOVE_TIME, planning.cleanJointConfig(milestone))
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig(eval("Q_IK_SEED_" + self.current_bin))
)
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
self.load_real_robot_state()
self.Tvacuum = se3.mul(self.robotModel.link("right_wrist").getTransform(), VACUUM_POINT_XFORM)
print WARNING_COLOR + str(self.object_com) + END_COLOR
self.object_com = vectorops.add(self.object_com, COM_ADJUSTMENT)
current_vacuum_point = se3.apply(self.Tvacuum, [0, 0, 0])
milestone = vectorops.add(current_vacuum_point, self.object_com)
milestone = vectorops.div(milestone, 2)
if DRY_RUN:
self.state = "MOVING_TO_GRASP_OBJECT"
else:
if self.right_arm_ik(milestone):
destination = self.robotModel.getConfig()
self.q_milestone = [destination[v] for v in self.right_arm_indices]
print WARNING_COLOR + "IK config for " + str(milestone) + ": " + str(
self.q_milestone
) + END_COLOR
motion.robot.right_mq.appendLinear(MOVE_TIME, planning.cleanJointConfig(self.q_milestone))
else:
print FAIL_COLOR + "Error: IK failed" + END_COLOR
sys.stdout.flush()
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig(eval("Q_IK_SEED_" + self.current_bin))
)
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
if self.right_arm_ik(self.object_com):
destination = self.robotModel.getConfig()
print WARNING_COLOR + "IK config for " + str(self.object_com) + ": " + str(
[destination[v] for v in self.right_arm_indices]
) + END_COLOR
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig([destination[v] for v in self.right_arm_indices])
)
self.state = "MOVING_TO_GRASP_OBJECT"
else:
print FAIL_COLOR + "Error: IK failed" + END_COLOR
sys.stdout.flush()
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig(eval("Q_IK_SEED_" + self.current_bin))
)
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
elif self.state == "MOVING_TO_GRASP_OBJECT":
if not motion.robot.right_mq.moving():
time.sleep(1)
self.state = "GRASP_OBJECT"
elif self.state == "GRASP_OBJECT":
move_target = se3.apply(self.Tvacuum, [0, 0, 0])
move_target[2] = move_target[2] - GRASP_MOVE_DISTANCE - (MEAN_OBJ_HEIGHT / 2)
if self.right_arm_ik(move_target):
self.turnOnVacuum()
destination = self.robotModel.getConfig()
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig([destination[v] for v in self.right_arm_indices])
)
else:
print FAIL_COLOR + "Error: IK failed" + END_COLOR
sys.stdout.flush()
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig(eval("Q_IK_SEED_" + self.current_bin))
)
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
self.wait_start_time = time.time()
self.state = "WAITING_TO_GRASP_OBJECT"
elif self.state == "WAITING_TO_GRASP_OBJECT":
if time.time() - self.wait_start_time > GRASP_WAIT_TIME:
self.state = "MOVE_UP_BEFORE_RETRACT"
elif self.state == "MOVE_UP_BEFORE_RETRACT":
move_target = se3.apply(self.Tvacuum, [0, 0, 0])
move_target[2] = move_target[2] + GRASP_MOVE_DISTANCE + (MEAN_OBJ_HEIGHT / 2)
if self.right_arm_ik(move_target):
self.turnOnVacuum()
destination = self.robotModel.getConfig()
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig([destination[v] for v in self.right_arm_indices])
)
else:
print FAIL_COLOR + "Error: IK failed" + END_COLOR
sys.stdout.flush()
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig(eval("Q_IK_SEED_" + self.current_bin))
)
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
self.state = "MOVE_TO_STOW_OBJECT"
elif self.state == "MOVE_TO_STOW_OBJECT":
motion.robot.right_mq.appendLinear(
MOVE_TIME, planning.cleanJointConfig(eval("Q_IK_SEED_" + self.current_bin))
)
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
if not DRY_RUN:
motion.robot.right_mq.appendLinear(MOVE_TIME, planning.cleanJointConfig(self.q_milestone))
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
for milestone in eval("Q_AFTER_GRASP_" + self.current_bin):
print "Moving to " + str(milestone)
motion.robot.right_mq.appendLinear(MOVE_TIME, planning.cleanJointConfig(milestone))
while motion.robot.right_mq.moving():
time.sleep(1)
time.sleep(1)
motion.robot.right_mq.appendLinear(MOVE_TIME, Q_STOW)
self.state = "MOVING_TO_STOW_OBJECT"
elif self.state == "MOVING_TO_STOW_OBJECT":
if not motion.robot.right_mq.moving():
self.state = "STOWING_OBJECT"
elif self.state == "STOWING_OBJECT":
self.turnOffVacuum()
self.wait_start_time = time.time()
self.state = "WAITING_FOR_SECURE_STOW"
elif self.state == "WAITING_FOR_SECURE_STOW":
if time.time() - self.wait_start_time > GRASP_WAIT_TIME:
self.state = "BIN_DONE" if SELECT_REAL_BIN else "DONE"
elif self.state == "BIN_DONE":
self.bin_state[self.current_bin]["done"] = True
self.current_bin = planning.selectBin(self.bin_state)
if self.current_bin is None:
self.state = "DONE"
else:
self.state = "START"
elif self.state == "DONE":
print "actual vacuum point: ", se3.apply(self.Tvacuum, [0, 0, 0])
else:
print FAIL_COLOR + "Unknown state" + END_COLOR
time.sleep(1)
except KeyboardInterrupt:
motion.robot.stopMotion()
sys.exit(0)
