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 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 moveRobot(self, movement=None):
print 'Moving robot'
global LEFT_ARM_INDICES
global RIGHT_ARM_INDICES
joint_ones = ['1', '!']
joint_twos = ['2', '@']
joint_threes = ['3', '#']
joint_fours = ['4', '$']
joint_fives = ['5', '%']
joint_sixes = ['6', '^']
joint_sevens = ['7', '&']
plus = ['1','2','3','4','5','6','7']
minus = ['!','@','#','$','%','^','&']
index = 0
factor = 0
adjustment = [0,0,0,0,0,0,0]
if movement in joint_ones:
index = 0
elif movement in joint_twos:
index = 1
elif movement in joint_threes:
index = 2
elif movement in joint_fours:
index = 3
elif movement in joint_fives:
index = 4
elif movement in joint_sixes:
index = 5
elif movement in joint_sevens:
index = 6
if movement in plus:
factor = 1
elif movement in minus:
factor = -1
adjustment[index] = factor*self.degreeChange*math.pi/180.0
currentSetup = self.controller.controller.getCommandedConfig()
if self.currentLimb == LEFT:
leftMilestone = [currentSetup[v] for v in LEFT_ARM_INDICES]
leftMilestone = vectorops.add(leftMilestone, adjustment)
adjustedMilestone = [currentSetup[i] for i in range(len(currentSetup))]
for i in range(len(LEFT_ARM_INDICES)):
adjustedMilestone[LEFT_ARM_INDICES[i]] = leftMilestone[i]
self.controller.controller.setLinear(adjustedMilestone, .1)
elif self.currentLimb == RIGHT:
rightMilestone = [currentSetup[v] for v in RIGHT_ARM_INDICES]
rightMilestone = vectorops.add(rightMilestone, adjustment)
adjustedMilestone = [currentSetup[i] for i in range(len(currentSetup))]
for i in range(len(RIGHT_ARM_INDICES)):
adjustedMilestone[RIGHT_ARM_INDICES[i]] = rightMilestone[i]
self.controller.controller.setLinear(adjustedMilestone, .1)
return
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 right_arm_ik_near_seed(self, right_target, ignore_elbow_up_constraint=True):
"""Solves IK to move the right arm to the specified
right_target ([x, y, z] in world space)
"""
self.load_real_robot_state()
self.set_model_right_arm(eval("Q_IK_SEED_" + self.current_bin))
oldRSquared = -1
q_ik = None
qmin, qmax = self.robotModel.getJointLimits()
for i in range(1000):
point2_local = vectorops.add(VACUUM_POINT_XFORM[1], [0.5, 0, 0])
point2_world = vectorops.add(right_target, [0, 0, -0.5])
goal1 = ik.objective(self.robotModel.link("right_wrist"), local=VACUUM_POINT_XFORM[1], world=right_target)
goal2 = ik.objective(self.robotModel.link("right_wrist"), local=point2_local, world=point2_world)
if ik.solve([goal1, goal2], tol=0.0001) and (self.elbow_up() or ignore_elbow_up_constraint):
q_vals = [self.robotModel.getConfig()[v] for v in self.right_arm_indices]
rSquared = vectorops.norm(vectorops.sub(q_vals, eval("Q_IK_SEED_" + self.current_bin)))
if oldRSquared < 0 or oldRSquared > rSquared:
oldRSquared = rSquared
q_ik = q_vals
print FAIL_COLOR + "right_arm_ik failed for " + str(right_target) + END_COLOR
if not (self.elbow_up() or ignore_elbow_up_constraint):
print FAIL_COLOR + str([self.robotModel.getConfig()[v] for v in self.right_arm_indices]) + END_COLOR
print FAIL_COLOR + "IK found but elbow wasn't up" + END_COLOR
if oldRSquared >= 0:
self.set_model_right_arm(q_ik)
return True
return False
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 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 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 solve_3R_forward_kinematics(q1,q2,q3,L1=1,L2=1,L3=1):
"""Returns a list of (x,y,theta) triples for each link
for a planar, 3R manipulator with link lengths L1, L2, L3.
It also returns the end effector transform."""
T1 = (0,0,q1)
dx1 = (L1*math.cos(T1[2]),L1*math.sin(T1[2]))
T2 = vectorops.add((T1[0],T1[1]),dx1)+[T1[2]+q2]
dx2 = (L2*math.cos(T2[2]),L2*math.sin(T2[2]))
T3 = vectorops.add((T2[0],T2[1]),dx2)+[T2[2]+q3]
dx3 = (L3*math.cos(T3[2]),L2*math.sin(T3[2]))
T4 = vectorops.add((T3[0],T3[1]),dx3)+[T3[2]]
return [T1,T2,T3,T4]
def solve_3R_forward_kinematics(q1, q2, q3, L1=1, L2=1, L3=1):
"""Returns a list of (x,y,theta) triples for each link
for a planar, 3R manipulator with link lengths L1, L2, L3.
It also returns the end effector transform."""
T1 = (0, 0, q1)
dx1 = (L1 * math.cos(T1[2]), L1 * math.sin(T1[2]))
T2 = vectorops.add((T1[0], T1[1]), dx1) + [T1[2] + q2]
dx2 = (L2 * math.cos(T2[2]), L2 * math.sin(T2[2]))
T3 = vectorops.add((T2[0], T2[1]), dx2) + [T2[2] + q3]
dx3 = (L3 * math.cos(T3[2]), L2 * math.sin(T3[2]))
T4 = vectorops.add((T3[0], T3[1]), dx3) + [T3[2]]
return [T1, T2, T3, T4]
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 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 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 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 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 move_gripper_upright(self,limb,moveVector,collisionchecker = None):
vertical = [0,0,0.1]
if self.active_limb == 'left':
gripperlink = self.left_gripper_link
else:
gripperlink = self.right_gripper_link
qcur = self.robot.getConfig()
vertical_in_gripper_frame = so3.apply(so3.inv(gripperlink.getTransform()[0]),vertical)
centerpos = se3.mul(gripperlink.getTransform(),left_gripper_center_xform)[1]
move_target = vectorops.add(centerpos,moveVector)
movegoal = ik.objective(gripperlink,local=[left_gripper_center_xform[1],vectorops.add(left_gripper_center_xform[1],vertical_in_gripper_frame)],world=[move_target,vectorops.add(move_target,vertical)])
sortedSolutions = self.get_ik_solutions([movegoal],[self.active_limb],qcur,validity_checker=collisionchecker,maxResults=1)
if len(sortedSolutions) == 0:
print "No upright-movement config found"
return False
self.robot.setConfig(sortedSolutions[0][0])
return True
def reward(self,state,action=None,actionargs={},actionresult=None):
"""Assesses the reward of the given action"""
res = None
for r in self.rewards:
if r.applicable(state,action,actionargs,actionresult):
if res == None:
res = r.value(state,action,actionargs,actionresult)
else:
res = vectorops.add(res,r.value(state,action,actionargs,actionresult))
return res
def right_arm_ik(self, right_target, ignore_elbow_up_constraint=True):
"""Solves IK to move the right arm to the specified
right_target ([x, y, z] in world space)
"""
self.load_real_robot_state()
self.set_model_right_arm(eval('Q_IK_SEED_' + self.current_bin))
qmin,qmax = self.robotModel.getJointLimits()
for i in range(1000):
point2_local = vectorops.add(VACUUM_POINT_XFORM[1], [.5, 0, 0])
point2_world = vectorops.add(right_target, [0, 0, -.5])
goal1 = ik.objective(self.robotModel.link('right_wrist'),local=VACUUM_POINT_XFORM[1],world=right_target)
goal2 = ik.objective(self.robotModel.link('right_wrist'),local=point2_local,world=point2_world)
if ik.solve([goal1, goal2],tol=0.0001) and (self.elbow_up() or ignore_elbow_up_constraint):
return True
print FAIL_COLOR + "right_arm_ik failed for " + str(right_target) + END_COLOR
if not (self.elbow_up() or ignore_elbow_up_constraint):
print FAIL_COLOR + str([self.robotModel.getConfig()[v] for v in self.right_arm_indices]) + END_COLOR
print FAIL_COLOR + "IK found but elbow wasn't up" + END_COLOR
return False
def sphere_collision_tests(x, radius, grid):
bmin = vectorops.sub(x, [radius] * len(x))
bmax = vectorops.add(x, [radius] * len(x))
imin = [max(0, int(v)) for v in bmin]
imax = [min(int(v), b - 1) for v, b in zip(bmax, workspace_size)]
for i in range(imin[0], imax[0]):
for j in range(imin[1], imax[1]):
for k in range(imin[2], imax[2]):
if vectorops.distanceSquared((i, j, k), x) < radius**2:
yield (i, j, k)
return
def bin_vantage_point(self,bin_name):
#TODO: remove me
world_center = self.bin_front_center(bin_name)
#20cm offset
world_offset = so3.apply(self.shelf_xform[0],[0,0,0.2])
return vectorops.add(world_center,world_offset)
#you may want to implement this. Returns the world position of the
#vantage point for viewing the bin. The visualizer will draw these points if
#'draw_bins' is turned to True.
#TODO:
return None
def gen_add(a,b):
"""a and b can be floats, integers, lists of floats/integers, or None.
If both are lists, must be of same length"""
if a == None: return b
if b == None: return a
if isinstance(a,(list,tuple)):
if isinstance(b,(list,tuple)):
return vectorops.add(a,b)
else:
return [ai + b for ai in a]
elif isinstance(b,(list,tuple)):
return [bi + a for bi in b]
else:
return a+b
def updateAllObjectives(self, frameIdx):
for i in range(0, len(self.objectives)):
obj = self.objectives[i]
frame1pos = self.data.getMarkerPosAtFrame(
frameIdx - 1, i
) #self.data.getFrame(frameIdx-1).viconMarkers[i].markerFramePos
frame2pos = self.data.getMarkerPosAtFrame(
frameIdx, i
) #self.data.getFrame(frameIdx).viconMarkers[i].markerFramePos
posChange = utils.calcChangeBetwFrames(frame1pos, frame2pos)
pos = vectorops.add(obj.globalpos, posChange)
obj.globalpos = pos
def planTransfer(world, objectIndex, hand, shift):
"""Plan a transfer path for the robot given in world, which is currently
holding the object indexed by objectIndex in the hand hand.
The desired motion should translate the object by shift without rotating
the object.
"""
globals = Globals(world)
obj = world.rigidObject(objectIndex)
cspace = TransferCSpace(globals, hand, obj)
robot = world.robot(0)
qmin, qmax = robot.getJointLimits()
#get the start config
q0 = robot.getConfig()
q0arm = [q0[i] for i in hand.armIndices]
if not cspace.feasible(q0arm):
print "Warning, arm start configuration is infeasible"
print "TODO: Complete 2.a to bypass this error"
raw_input()
return None
#TODO: get the ungrasp config using an IK solver
qungrasp = None
qungrasparm = None
print "TODO: Complete 2.b to find a feasible ungrasp config"
raw_input()
solver = hand.ikSolver(robot, vectorops.add(obj.getTransform()[1], shift),
(0, 0, 1))
#plan the transfer path between q0arm and qungrasparm
print "Planning transfer motion to ungrasp config..."
MotionPlan.setOptions(connectionThreshold=5.0, perturbationRadius=0.5)
planner = MotionPlan(cspace, 'sbl')
planner.setEndpoints(q0arm, qungrasparm)
#TODO: do the planning
print "TODO: Complete 2.c to find a feasible transfer path"
raw_input()
#lift arm path to whole configuration space path
path = []
for qarm in planner.getPath():
path.append(q0[:])
for qi, i in zip(qarm, hand.armIndices):
path[-1][i] = qi
qpostungrasp = hand.open(qungrasp, 1.0)
return path + [qpostungrasp]
def planTransfer(world,objectIndex,hand,shift):
"""Plan a transfer path for the robot given in world, which is currently
holding the object indexed by objectIndex in the hand hand.
The desired motion should translate the object by shift without rotating
the object.
"""
globals = Globals(world)
obj = world.rigidObject(objectIndex)
cspace = TransferCSpace(globals,hand,obj)
robot = world.robot(0)
qmin,qmax = robot.getJointLimits()
#get the start config
q0 = robot.getConfig()
q0arm = [q0[i] for i in hand.armIndices]
if not cspace.feasible(q0arm):
print "Warning, arm start configuration is infeasible"
print "TODO: Complete 2.a to bypass this error"
raw_input()
return None
#TODO: get the ungrasp config using an IK solver
qungrasp = None
qungrasparm = None
print "TODO: Complete 2.b to find a feasible ungrasp config"
raw_input()
solver = hand.ikSolver(robot,vectorops.add(obj.getTransform()[1],shift),(0,0,1))
#plan the transfer path between q0arm and qungrasparm
print "Planning transfer motion to ungrasp config..."
MotionPlan.setOptions(connectionThreshold=5.0,perturbationRadius=0.5)
planner = MotionPlan(cspace,'sbl')
planner.setEndpoints(q0arm,qungrasparm)
#TODO: do the planning
print "TODO: Complete 2.c to find a feasible transfer path"
raw_input()
#lift arm path to whole configuration space path
path = []
for qarm in planner.getPath():
path.append(q0[:])
for qi,i in zip(qarm,hand.armIndices):
path[-1][i] = qi
qpostungrasp = hand.open(qungrasp,1.0)
return path + [qpostungrasp]
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 load_world():
world = robotsim.WorldModel()
print "Loading reflex hands"
# world.loadElement(os.path.join(model_dir,"reflex.rob"))
world.loadElement(os.path.join(model_dir,"reflex_col_with_moving_base.rob"))
print "Loading plane model..."
world.loadElement(os.path.join(model_dir,"plane.env"))
R,t = world.robot(0).link(0).getParentTransform()
R_reorient = so3.rotation([1,0,0], math.pi/2)
offset = (0,0.15,0.1)
R = so3.mul(R,R_reorient)
t = vectorops.add(t, offset)
world.robot(0).link(0).setParentTransform(R,t)
for i in range(world.robot(0).numLinks()):
world.robot(0).link(i).geometry().setCollisionMargin(0)
return world
def bin_vantage_point(self,bin_name):
#you may want to implement this. Returns the world position of the
#vantage point for viewing the bin. The visualizer will draw these points if
#'draw_bins' is turned to True.
# determine the vantage point by moving the front center point back from the shelf
# start by getting the front center in world coordinates
front_center_world = self.bin_front_center(bin_name)
if not front_center_world:
return None
# move back into local coordinates since the local z frame is aligned with the offset direction
front_center_local = se3.apply(se3.inv(self.shelf_xform), front_center_world)
# now offset the the front center point along z to get the vantage point
vantage_point_local = vectorops.add(front_center_local, [ 0, 0, vantage_point_offset ])
# transform back into world coordinates
vantage_point_world = se3.apply(self.shelf_xform, vantage_point_local)
return vantage_point_world
def add(self,value,weight=1.0):
"""Accumulates a new value, with an optional weight factor. The
weight can either be a float or a list. In the latter case it is
a per-element weight."""
assert len(value) == len(self.average)
if not hasattr(weight,'__iter__'):
weight = [weight]*len(value)
newcount = vectorops.add(self.count,weight)
oldweight = [a/b for (a,b) in zip(self.count,newcount)]
newweight = [1.0/b for b in newcount]
#oldaverage = self.average
self.average = [(1.0-w)*a + w*b for (a,b,w) in zip(self.average,value,newweight)]
#variance = E[vv^T] - E[v]E[v^T]
#variance(n) = 1/n sum_{1 to n+1} v_i*v_i^T - average(n)*average(n)^T
#variance(n+1) = 1/(n+1)sum_{1 to n+1} v_i*v_i^T - average(n+1)*average(n+1)^T
# = 1/(n+1) sum_{1 to n} v_i*v_i^T + 1/(n+1) v_{n+1}*v_{n+1}^T - average(n+1)*average(n+1)^T
# = 1/(n+1)(n variance(n) + average(n)*average(n)^T) - average(n+1)*average(n+1)^T + 1/(n+1) v_{n+1}*v_{n+1}^T
# = n/(n+1) variance(n) + 1/(n+1)(v_{n+1}*v_{n+1}^T + average(n)*average(n)^T) - average(n+1)*average(n+1)^T
#temp1 = matrixops.add(matrixops.outer(oldaverage,oldaverage),matrixops.outer(value,value))
#temp2 = matrixops.madd(matrixops.mul(oldweight,self.variance),temp1,newweight)
#self.variance = matrixops.add(temp2,matrixops.outer(self.average,self.average)
self.count = newcount
def add(self,value,weight=1.0):
"""Accumulates a new value, with an optional weight factor. The
weight can either be a float or a list. In the latter case it is
a per-element weight."""
assert len(value) == len(self.average)
if not hasattr(weight,'__iter__'):
weight = [weight]*len(value)
newcount = vectorops.add(self.count,weight)
oldweight = [a/b for (a,b) in zip(self.count,newcount)]
newweight = [1.0/b for b in newcount]
#oldaverage = self.average
self.average = [(1.0-w)*a + w*b for (a,b,w) in zip(self.average,value,newweight)]
#variance = E[vv^T] - E[v]E[v^T]
#variance(n) = 1/n sum_{1 to n+1} v_i*v_i^T - average(n)*average(n)^T
#variance(n+1) = 1/(n+1)sum_{1 to n+1} v_i*v_i^T - average(n+1)*average(n+1)^T
# = 1/(n+1) sum_{1 to n} v_i*v_i^T + 1/(n+1) v_{n+1}*v_{n+1}^T - average(n+1)*average(n+1)^T
# = 1/(n+1)(n variance(n) + average(n)*average(n)^T) - average(n+1)*average(n+1)^T + 1/(n+1) v_{n+1}*v_{n+1}^T
# = n/(n+1) variance(n) + 1/(n+1)(v_{n+1}*v_{n+1}^T + average(n)*average(n)^T) - average(n+1)*average(n+1)^T
#temp1 = matrixops.add(matrixops.outer(oldaverage,oldaverage),matrixops.outer(value,value))
#temp2 = matrixops.madd(matrixops.mul(oldweight,self.variance),temp1,newweight)
#self.variance = matrixops.add(temp2,matrixops.outer(self.average,self.average)
self.count = newcount
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 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
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
calibrationConfigs = []
trueObservations = []
observations = []
for obs in xrange(numObs):
q0 = [random.uniform(a,b) for (a,b) in zip(qmin,qmax)]
#don't move head
for i in range(13):
q0[i] = 0
trueCalibrationConfigs.append(q0)
trueCalibrationConfigs=resource.get("calibration.configs",default=trueCalibrationConfigs,type="Configs",description="Calibration configurations",world=world)
for q0 in trueCalibrationConfigs:
robot.setConfig(q0)
obs0 = se3.mul(se3.inv(lc.getTransform()),lm.getTransform())
dq = [random.uniform(-jointEncoderError,jointEncoderError) for i in range(len(q0))]
dobs = (so3.from_moment([random.uniform(-sensorErrorRads,sensorErrorRads) for i in range(3)]),[random.uniform(-sensorErrorMeters,sensorErrorMeters) for i in range(3)])
calibrationConfigs.append(vectorops.add(q0,dq))
observations.append(se3.mul(obs0,dobs))
trueObservations.append(obs0)
if DO_VISUALIZATION:
rgroup = coordinates.addGroup("calibration ground truth")
rgroup.addFrame("camera link",worldCoordinates=pc.getTransform())
rgroup.addFrame("marker link",worldCoordinates=pm.getTransform())
rgroup.addFrame("camera (ground truth)",parent="camera link",relativeCoordinates=Tc0)
rgroup.addFrame("marker (ground truth)",parent="marker link",relativeCoordinates=Tm0)
for i,(obs,obs0) in enumerate(zip(observations,trueObservations)):
rgroup.addFrame("obs"+str(i)+" (ground truth)",parent="camera (ground truth)",relativeCoordinates=obs0)
rgroup.addFrame("obs"+str(i)+" (from camera)",parent="camera (ground truth)",relativeCoordinates=obs)
vis.add("world",world)
for i,q in enumerate(calibrationConfigs):
vis.add("config"+str(i),q)
cmd = parts[0]
args = parts[1:]
if cmd == 'load' and len(args) == 1:
kb.target_object = args[0]
kb.object_xforms[kb.target_object] = se3.identity()
elif cmd == 'move' and len(args) <= 3:
(x, y, z) = map(float, args) + ([0] * (3 - len(args)))
xform = kb.object_xforms[kb.target_object]
kb.object_xforms[kb.target_object] = (xform[0], [ x, y, z ])
elif cmd == 'mover' and len(args) <= 3:
(x, y, z) = map(float, args) + ([0] * (3 - len(args)))
xform = kb.object_xforms[kb.target_object]
kb.object_xforms[kb.target_object] = (xform[0], vectorops.add(xform[1], [ x, y, z ]))
elif cmd == 'rot' and len(args) <= 3:
(rx, ry, rz) = map(lambda a: float(a) * 3.141592/180, args) + ([0] * (3 - len(args)))
xform = kb.object_xforms[kb.target_object]
kb.object_xforms[kb.target_object] = (
reduce(so3.mul, [ so3.rotation(v, a) for (v, a) in zip([ [1,0,0], [0,1,0], [0,0,1] ], [ rx, ry, rz ]) ]),
xform[1]
)
elif cmd == 'rotl' and len(args) <= 3:
(rx, ry, rz) = map(float, args) + ([0] * (3 - len(args)))
xform = kb.object_xforms[kb.target_object]
kb.object_xforms[kb.target_object] = (
reduce(so3.mul, [ xform[0] ] + [ so3.rotation(v, a) for (v, a) in zip([ [1,0,0], [0,1,0], [0,0,1] ], [ rx, ry, rz ]) ]),
xform[1]
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)
def drawRobotGL(self, q):
glColor3f(0, 0, 1)
newc = vectorops.add(self.robot.center, q)
c = Circle(newc[0], newc[1], self.robot.radius)
c.drawGL()
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 integrate(self,x,d):
"""For Lie groups, returns the point that would be arrived at via
integrating the difference vector d starting from x. Must satisfy
the relationship a = integrate(b,difference(a,b)). In Cartesian
spaces it is x+d"""
return vectorops.add(x,d)
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. Must change self.active_grasp to the
selected grasp.
If successful, sends the motion to the low-level controller and
returns True.
Otherwise, does not modify the low-level controller and returns False.
"""
self.waitForMove()
self.controller.commandGripper(self.active_limb,[1])
grasps = self.knowledge.grasp_xforms(object)
qmin,qmax = self.robot.getJointLimits()
#get the end of the commandGripper motion
qcmd = self.controller.getCommandedConfig()
self.robot.setConfig(qcmd)
set_model_gripper_command(self.robot,self.active_limb,[1])
qcmd = self.robot.getConfig()
#solve an ik solution to one of the grasps
grasp_goals = []
pregrasp_goals = []
pregrasp_shift = [0,0,0.05]
for (grasp,gxform) in grasps:
if self.active_limb == 'left':
Tg = se3.mul(gxform,se3.inv(left_gripper_center_xform))
goal = ik.objective(self.left_gripper_link,R=Tg[0],t=Tg[1])
Tpg = se3.mul(gxform,se3.inv((left_gripper_center_xform[0],vectorops.add(left_gripper_center_xform[1],pregrasp_shift))))
pregoal = ik.objective(self.left_gripper_link,R=Tpg[0],t=Tpg[1])
else:
Tg = se3.mul(gxform,se3.inv(right_gripper_center_xform))
goal = ik.objective(self.right_gripper_link,R=Tg[0],t=Tg[1])
Tpg = se3.mul(gxform,se3.inv((right_gripper_center_xform[0],vectorops.add(right_gripper_center_xform[1],pregrasp_shift))))
pregoal = ik.objective(self.right_gripper_link,R=Tpg[0],t=Tpg[1])
grasp_goals.append(goal)
pregrasp_goals.append(pregoal)
sortedSolutions = self.get_ik_solutions(pregrasp_goals,[self.active_limb]*len(grasp_goals),qcmd)
if len(sortedSolutions)==0: return False
#prototyping hack: move straight to target
if SKIP_PATH_PLANNING:
for pregrasp in sortedSolutions:
graspIndex = pregrasp[1]
gsolns = self.get_ik_solutions([grasp_goals[graspIndex]],[self.active_limb],pregrasp[0],maxResults=1)
if len(gsolns)==0:
print "Couldn't find grasp config for pregrasp? Trying another"
else:
self.waitForMove()
self.sendPath([pregrasp[0],gsolns[0][0]])
self.active_grasp = grasps[graspIndex][0]
return True
print "Planning failed"
return False
for solution in sortedSolutions:
path = self.planner.plan(qcmd,solution[0])
graspIndex = solution[1]
if path != None:
#now solve for grasp
gsolns = self.get_ik_solutions([grasp_goals[graspIndex]],[self.active_limb],path[-1],maxResults=1)
if len(gsolns)==0:
print "Couldn't find grasp config??"
else:
self.waitForMove()
self.sendPath(path+[gsolns[0][0]])
self.active_grasp = grasps[graspIndex][0]
return True
print "Planning failed"
return False
def drawRobotGL(self,q):
glColor3f(0,0,1)
newc = vectorops.add(self.robot.center,q)
c = Circle(newc[0],newc[1],self.robot.radius)
c.drawGL()
def bin_vantage_point(self,bin_name):
world_center = self.bin_front_center(bin_name)
# Vantage point has 20cm offset from bin center
world_offset = so3.apply(ground_truth_shelf_xform[0],[0,0,0.2])
return vectorops.add(world_center,world_offset)
def bin_vantage_point(self,bin_name):
world_center = self.bin_front_center(bin_name)
#20cm offset
world_offset = so3.apply(self.shelf_xform[0],[0,0,0.2])
return vectorops.add(world_center,world_offset)
