def move_to_order_bin(self,object):
"""Sets the robot's configuration so the gripper is over the order bin
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
#TODO: remove me
left_target = se3.apply(order_bin_xform,[0.0,0.2,order_bin_bounds[1][2]+0.1])
right_target = se3.apply(order_bin_xform,[0.0,-0.2,order_bin_bounds[1][2]+0.1])
qmin,qmax = self.robot.getJointLimits()
for i in range(100):
if self.active_limb == 'left':
q = baxter_rest_config[:]
for j in self.left_arm_indices:
q[j] = random.uniform(qmin[j],qmax[j])
#goal = ik.objective(self.left_gripper_link,local=[vectorops.sub(left_gripper_center_xform[1],[0,0,0.1]),left_gripper_center_xform[1]],world=[vectorops.add(target,[0,0,0.1]),target])
goal = ik.objective(self.left_gripper_link,local=left_gripper_center_xform[1],world=left_target)
if ik.solve(goal,tol=0.1):
self.config = self.robot.getConfig()
return True
self.config = self.robot.getConfig()
else:
q = baxter_rest_config[:]
for j in self.right_arm_indices:
q[j] = random.uniform(qmin[j],qmax[j])
#goal = ik.objective(self.right_gripper_link,local=[vectorops.sub(right_gripper_center_xform[1],[0,0,0.1]),right_gripper_center_xform[1]],world=[vectorops.add(target,[0,0,0.1]),target])
goal = ik.objective(self.right_gripper_link,local=right_gripper_center_xform[1],world=right_target)
if ik.solve(goal,tol=0.1):
self.config = self.robot.getConfig()
return True
self.config = self.robot.getConfig()
#TODO: put my code here
return False
def getJacobian(self, q):
self.robot.setConfig(q)
J = None
if self.taskType == 'po':
J = self.link.getJacobian(self.localPosition)
elif self.taskType == 'position':
J = self.link.getPositionJacobian(self.localPosition)
elif self.taskType == 'orientation':
J = self.link.getOrientationJacobian()
else:
raise ValueError("Invalid taskType " + self.taskType)
J = np.array(J)
#check if relative transform task, modify Jacobian accordingly
if self.baseLinkNo >= 0:
T = self.link.getTransform()
Tb = self.baseLink.getTransform()
Tbinv = se3.inv(Tb)
pb = se3.apply(Tbinv, se3.apply(T, self.localPosition))
if self.taskType == 'po':
Jb = self.baseLink.getJacobian(pb)
elif self.taskType == 'position':
Jb = self.baseLink.getPositionJacobian(pb)
elif self.taskType == 'orientation':
Jb = self.baseLink.getOrientationJacobian()
Jb = np.array(Jb)
#subtract out jacobian w.r.t. baseLink
J -= Jb
if self.activeDofs != None:
J = select_cols(J, self.activeDofs)
return J
def drawGL(self, q):
x = self.getSensedValue(q)
xdes = self.xdes
if self.baseLinkNo >= 0:
Tb = self.baseLink.getTransform()
if self.taskType == "position":
x = se3.apply(Tb, x)
xdes = se3.apply(Tb, xdes)
elif self.taskType == "po":
x = se3.mul(Tb, x)
xdes = se3.mul(Tb, xdes)
glPointSize(6)
glEnable(GL_POINT_SMOOTH)
glBegin(GL_POINTS)
if self.taskType == "position":
glColor3f(1, 0, 0) #red
glVertex3fv(xdes)
glColor3f(1, 0.5, 0) #orange
glVertex3fv(x)
elif self.taskType == "po":
glColor3f(1, 0, 0) #red
glVertex3fv(xdes[1])
glColor3f(1, 0.5, 0) #orange
glVertex3fv(x[1])
glEnd()
def display(self):
#draw the world
self.world.drawGL()
lh = Hand('l')
rh = Hand('r')
glDisable(GL_LIGHTING)
glPointSize(5.0)
glDisable(GL_DEPTH_TEST)
glBegin(GL_POINTS)
glColor3f(0, 1, 0)
glVertex3fv(
se3.apply(
self.world.robot(0).link(lh.link).getTransform(),
lh.localPosition1))
glVertex3fv(
se3.apply(
self.world.robot(0).link(rh.link).getTransform(),
rh.localPosition1))
glColor3f(0, 0, 1)
glVertex3fv(
se3.apply(
self.world.robot(0).link(lh.link).getTransform(),
lh.localPosition2))
glVertex3fv(
se3.apply(
self.world.robot(0).link(rh.link).getTransform(),
rh.localPosition2))
glColor3f(1, 0, 0)
glVertex3fv(self.world.rigidObject(0).getTransform()[1])
glEnd()
glEnable(GL_DEPTH_TEST)
def display(self):
#Put your display handler here
#the current example draws the simulated world in grey and the
#commanded configurations in transparent green
self.sim.updateWorld()
self.world.drawGL()
#draw tendons
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
glLineWidth(4.0)
glColor3f(0,1,1)
glBegin(GL_LINES)
for i in range(3):
b1 = self.sim.getBody(self.handsim.model.robot.getLink(self.handsim.model.proximal_links[i]))
b2 = self.sim.getBody(self.handsim.model.robot.getLink(self.handsim.model.distal_links[i]))
glVertex3f(*se3.apply(b1.getTransform(),self.handsim.tendon1_local))
glVertex3f(*se3.apply(b2.getTransform(),self.handsim.tendon2_local))
glEnd()
glLineWidth(1)
glEnable(GL_DEPTH_TEST)
glEnable(GL_LIGHTING)
#draw commanded configurations
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0,1,0,0.5])
for i in xrange(self.world.numRobots()):
r = self.world.robot(i)
q = self.sim.getController(i).getCommandedConfig()
r.setConfig(q)
r.drawGL(False)
glDisable(GL_BLEND)
def getJacobian(self, q):
self.robot.setConfig(q)
J = None
if self.taskType == 'po':
J = self.link.getJacobian(self.localPosition)
elif self.taskType == 'position':
J = self.link.getPositionJacobian(self.localPosition)
elif self.taskType == 'orientation':
J = self.link.getOrientationJacobian()
else:
raise ValueError("Invalid taskType "+self.taskType)
J = np.array(J)
#check if relative transform task, modify Jacobian accordingly
if self.baseLinkNo >= 0:
T = self.link.getTransform()
Tb = self.baseLink.getTransform()
Tbinv = se3.inv(Tb)
pb = se3.apply(Tbinv,se3.apply(T,self.localPosition))
if self.taskType == 'po':
Jb = self.baseLink.getJacobian(pb)
elif self.taskType == 'position':
Jb = self.baseLink.getPositionJacobian(pb)
elif self.taskType == 'orientation':
Jb = self.baseLink.getOrientationJacobian()
Jb = np.array(Jb)
#subtract out jacobian w.r.t. baseLink
J -= Jb
if self.activeDofs!=None:
J = select_cols(J,self.activeDofs)
return J
def move_to_grasp_object(self,object):
#Select appropriate limb and link
self.active_limb = self.limb_selector("gripper")
#Extract the contents of the bin
binContents = self.knowledge.bin_contents[self.current_bin]
#Extract the list of possible grasp points
graspList = binContents[0].info.grasps
#Loop through grasp list and attempt to solve inverse kinematics
for graspIndex in range(0, len(graspList)):
#Compute transform from grasp point to bin to world
graspTransform = graspList[graspIndex].grasp_xform
binCoordinate = se3.apply(graspTransform,[0,0,0])
worldCoordinate = se3.apply(binContents[0].xform, binCoordinate)
#Compute objective
objective = ik.objective(self.active_limb, local=[0,0,0], world=worldCoordinate)
#Attempt to solve inverse kinematics
if ik.solve(objective):
self.config = self.robot.getConfig()
return True
else:
return False
def move_to_order_bin(self,object):
"""Sets the robot's configuration so the gripper is over the order bin
If successful, sends the motion to the low-level controller and
returns True.
Otherwise, does not modify the low-level controller and returns False.
"""
left_target = se3.apply(order_bin_xform,[0.0,0.2,order_bin_bounds[1][2]+0.1])
right_target = se3.apply(order_bin_xform,[0.0,-0.2,order_bin_bounds[1][2]+0.1])
qcmd = self.controller.getCommandedConfig()
for i in range(100):
if self.active_limb == 'left':
goal = ik.objective(self.left_gripper_link,local=left_gripper_center_xform[1],world=left_target)
else:
goal = ik.objective(self.right_gripper_link,local=right_gripper_center_xform[1],world=right_target)
#set IK solver initial configuration
if i==0:
self.robot.setConfig(qcmd)
else:
self.randomize_limb_position(self.active_limb)
#solve
if ik.solve(goal,tol=0.1):
if self.planner.check_collision_free('left'):
self.controller.setMilestone(self.robot.getConfig())
self.active_limb = 'left'
return True
return False
def drawGL(self):
#draw tendons
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
glLineWidth(4.0)
glColor3f(0, 1, 1)
glBegin(GL_LINES)
for i in range(3):
b1 = self.sim.body(
self.model.robot.link(self.model.proximal_links[i]))
b2 = self.sim.body(
self.model.robot.link(self.model.distal_links[i]))
glVertex3f(*se3.apply(b1.getTransform(), self.tendon1_local))
glVertex3f(*se3.apply(b2.getTransform(), self.tendon2_local))
if self.ext_forces[i]:
glColor3f(0, 1, 0)
b = self.sim.body(
self.model.robot.link(self.model.proximal_links[i]))
print "Applying ext force", self.EXT_F, "to", i
glVertex3f(*se3.apply(b.getTransform(), self.EXT_F_DISP))
glVertex3f(
*se3.apply(b.getTransform(),
vectorops.sub(self.EXT_F_DISP, self.EXT_F)))
glEnd()
glLineWidth(1)
glEnable(GL_DEPTH_TEST)
glEnable(GL_LIGHTING)
def drawGL(self,q): x = self.getSensedValue(q) xdes = self.xdes if self.baseLinkNo >= 0: Tb = self.baseLink.getTransform() if self.taskType == "position": x = se3.apply(Tb,x) xdes = se3.apply(Tb,xdes) elif self.taskType == "po": x = se3.mul(Tb,x) xdes = se3.mul(Tb,xdes) glPointSize(6) glEnable(GL_POINT_SMOOTH) glBegin(GL_POINTS) if self.taskType == "position": glColor3f(1,0,0) #red glVertex3fv(xdes) glColor3f(1,0.5,0) #orange glVertex3fv(x) elif self.taskType == "po": glColor3f(1,0,0) #red glVertex3fv(xdes[1]) glColor3f(1,0.5,0) #orange glVertex3fv(x[1]) glEnd()
def goToOrderBin(self,limb,fromWhere):
qcmd = self.controller.getCommandedConfig()
left_target = se3.apply(order_bin_xform,[0.0,0.2,order_bin_bounds[1][2]+0.1])
right_target = se3.apply(order_bin_xform,[0.0,-0.2,order_bin_bounds[1][2]+0.1])
if limb == 'left':
placegoal = ik.objective(self.left_gripper_link,local=baxter.left_gripper_center_xform[1],world=left_target)
else:
placegoal = ik.objective(self.right_gripper_link,local=baxter.right_gripper_center_xform[1],world=right_target)
sortedSolutions = self.get_ik_solutions([placegoal],[limb],qcmd,tol=0.1)
if len(sortedSolutions) == 0:
print "Failed to find placement config"
for solution in sortedSolutions:
if limb == 'left':
path = self.planner.plan(qcmd, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3717782274123387, -0.674937510436548, -0.33948575074327003, 2.1672104893835455, -0.08734593872357976, 0.0, 0.12728394408809324, 9.587078444893871e-09, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0600000000000005, 0.0, 2.31, 0.0, 0.0, 0.28, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
else:
path = self.planner.plan(qcmd, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.1399999999999997, 0.0, 2.32, 0.0, 0.0, 0.32, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.13866956622769833, -0.6983335926844152, 0.43471512229342457, 2.241413168181005, 0.09879187313355131, 0.0, 0.30929795433686946, 0.13997178733833623, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
#path = self.planner.plan(qcmd,solution[0])
if path != None:
if(fromWhere == 'nofile'):
self.waitForMove()
self.sendPath(path)
else:
self.waitForMove()
self.sendPath(path)
self.printPath(path,'cached_motions/'+limb+'_from_bin'+fromWhere+'_2home')
return True
print "Planning failed"
return False
def icp(object,scene): #return se3.identity() """Computes a rotation and translation of the object to the scene using the ICP algorithm. Returns the transform (R,t) in Klamp't se3 format. """ kd = KDTree(scene) print "kd tree done" #TODO: implement me # Sample both maps to make the closest point computations faster # Compute the minimum distance between the points # Reject the outliers, for example, using a threshold # Compute the R and t matrices. The procedure can be similar as the one for the # 2D images. #ret = se3.identity() #return ret #ret[1][2]+=0.2 #ret[1][1]+=0.3 #return ret sampled = random.sample(object, 1000) threshold = 0.75 net = se3.identity() #net =[net[0], (0.2,0.05,0)] #return net sampled = [se3.apply(net, o) for o in sampled] #print object[0] for _ in xrange(10): print "start finding correspondances" correspondings = [] for i in sampled: try: (dist, idx) = kd.query(i) except: print i raise Exception() correspondings.append((i, scene[idx], dist)) correspondings.sort(key=lambda x: x[2]) correspondings = correspondings[0:int(len(correspondings)*threshold)] #print "\n".join(map(str,correspondings)) print "done finding correspondances" object_points = [i[0] for i in correspondings] scene_points = [i[1] for i in correspondings] (R,t) = one_round_icp(scene_points, object_points, 'list') #(R,t) = one_round_icp(object_points, scene_points, 'matrix') sampled = [se3.apply((R,t),i) for i in sampled] net = se3.mul((R,t), net) #print R,t #print net return net
def ApplyICP(object, scene):
outR = [];
outT = [];
SSEm1 = 1000000000
for i in range(0,5):
SSE = 1000000
SSE_min = 10000000
SSE_max = 10000000
#run the ICP
R,t,mR,mt = icp(object, scene)
if len(R) > 1:
#apply the ICP results to the model
maxModel = [se3.apply((R,t),p) for p in object]
maxSample = random.sample(object,int(float(len(object))*0.01))
SSE_max = sum_of_squared_errors(maxSample,scene)
elif R == -1:
SSE_max = 10000000
if len(mR) > 1:
minModel = [se3.apply((mR,mt),p) for p in object]
minSample = random.sample(object,int(float(len(object))*0.01))
SSE_min = sum_of_squared_errors(minSample,scene)
else:
SSE_min = 10000000
if SSE_min == 10000000 and SSE_max == 10000000:
SSE = 1000000
break
elif SSE_max <= SSE_min:
object = maxModel
print SSE_max
SSE = SSE_max
tempR = R
tempT = t
else:
object = minModel
print SSE_min
SSE = SSE_min
tempR = mR
tempT = mt
if SSEm1 > SSE:
SSEm1 = SSE
outR = tempR
outT = tempT
print SSE
return outR, outT
def move_to_order_bin(self,object):
"""Sets the robot's configuration so the gripper is over the order bin
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
# find a point a little (0.5 meters) above the order bin)
aboveOrderBinLocation = se3.apply(order_bin_xform, [0, 0, 0])
aboveOrderBinLocation[2] = aboveOrderBinLocation[2] + 0.5
oldConfig = self.robot.getConfig()
# figure out what gripper is holding the object
activeGripper = self.left_gripper_link
if self.active_limb == "Right":
activeGripper = self.right_gripper_link
goal = ik.objective(activeGripper, local=[0, 0, 0], world=aboveOrderBinLocation)
if ik.solve(goal):
self.config = self.robot.getConfig();
return True
#self.robot.setConfig(oldConfig)
print "IK solver failed to find a a point above the order bin."
return False
def move_to_order_bin(self,object):
"""Sets the robot's configuration so the gripper is over the order bin
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
#TODO: put my code here
#gohere
'''
bin_name = object.bin_name
pointInFrontOfBin = self.knowledge.bin_front_center(bin_name)
goal = ik.objective(self.active_limb,R=[0,1,0,0,0,1,1,0,0],t=pointInFrontOfBin)
self.config = self.robot.getConfig()
return ik.solve(goal)
'''
self.robot.setConfig(self.originalConfig)
self.config = self.originalConfig
orderBinLocalPoint = [(order_bin_bounds[0][i] + order_bin_bounds[1][i])/2 for i in range(3)]
orderBinLocalPoint[2] = .7
gripper = self.active_limb
orderBinWorldPoint = se3.apply(order_bin_xform,orderBinLocalPoint)
goal = ik.objective(gripper,R=[0,1,0,1,0,0,0,0,-1],t=orderBinWorldPoint)
iterations = 0
while iterations!=1000:
iterations+=1
if ik.solve(goal,tol=.1):
self.config = self.robot.getConfig()
return True
print "having trouble putting in order bin"
pointAroundBinWorldPoint = [orderBinWorldPoint[i] + random.random()*.1 for i in range(3)]
goal = ik.objective(gripper,R=[0,1,0,1,0,0,0,0,-1],t=pointAroundBinWorldPoint)
self.robot.setConfig(self.originalConfig)
self.config = self.originalConfig
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.
"""
#TODO: put my code here
graspToWorldTransforms = self.knowledge.grasp_xforms(object)
grasps_in_world_coords = [se3.apply(transform,[0,0,0]) for transform in graspToWorldTransforms]
if self.active_limb == self.left_camera_link:
gripper = self.left_gripper_link
print "left gripper"
if self.active_limb == self.right_camera_link:
gripper = self.left_gripper_link
print "right gripper"
goals = [ik.objective(gripper,R=graspToWorldTransforms[i][0],t=grasps_in_world_coords[i]) for i in range(len(graspToWorldTransforms))]
iterations = 0
while iterations!= 1000:
iterations += 1
for goal in goals:
if ik.solve(goal,tol=.01):
self.config = self.robot.getConfig()
return True
print "having trouble grasping"
if iterations % 500 == 0:
a = 5
goals = [ik.objective(gripper,R=graspToWorldTransforms[i][0],t=grasps_in_world_coords[i]) for i in range(len(graspToWorldTransforms))]
return False
def main():
"""
Main loop. Run ICP on the given model and scene file, display the results.
"""
model_full = get_reconstructed_model(model_file, True)
scene_full = get_raw_depth(depth_file)
# segment the scene
print
print ".................Segmenting data................."
sampled_scene = segmentScene(model_full["positions"], scene_full)
# run the ICP
print ".................Done segmenting................."
print ".................Running ICP....................."
if len(sampled_scene) > 0:
R, t = icp(sampled_scene[0], model_full["positions"], 0)
else:
R, t = icp(scene_full, model_full["positions"], 0.05)
print ".................ICP finished...................."
# apply the ICP results to the model
transformed_points = [se3.apply((R, t), p) for p in model_full["positions"]]
model_full["positions"] = transformed_points
# visualize the results
opengl_plot = OpenGLPlot(model_full, scene_full)
opengl_plot.initialize_main_loop()
def move_to_order_bin(self,object):
"""Sets the robot's configuration so the gripper is over the order bin
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
print
print 'Trying to move over order bin'
world_goal_xyz = se3.apply(order_bin_xform,[0,0,0.75])
cnt = 0;
while(cnt < 100):
#pick a random starting config for the left link and try to solve the ik problem
config = self.robot.getConfig()
for ind in self.left_arm_indices:
config[ind] = random.uniform(0,3);
self.robot.setConfig(config);
goal = ik.objective(self.left_gripper_link,local=([0,0,0]),world=(world_goal_xyz))
if ik.solve(goal,tol = 0.05): #lower tol because the bin is huge
self.active_limb = 'Left'
self.config = self.robot.getConfig()
return True
cnt = cnt + 1
return False
def main():
for x in range (0,len(obs)):
model_files = ["data/models/"+o+"_model.json" for o in objects]
depth_file = "data/processed_depth/"+obs[x]+".json"
print depth_file
models = [get_reconstructed_model(f,True) for f in model_files]
scene_full = get_raw_depth(depth_file)
errors = []
for i in range(len(objects)):
print model_files[i]
R,t = ApplyICP(models[i]['positions'],scene_full)
#apply the ICP results to the model
if len(R) != 0:
transformed_points = [se3.apply((R,t),p) for p in models[i]['positions']]
errors.append(matching_error(transformed_points,scene_full))
print "Object",objects[i],"matching error:",errors[-1]
models[i]['positions'] = transformed_points
else:
print "ERROR"
print depth_file
print "END"
print ""
#pick one of the models to visualize
visualization_model = 2
opengl_plot = OpenGLPlot(models[visualization_model], scene_full)
opengl_plot.initialize_main_loop()
def drawGL(self, q):
x = self.getSensedValue(q)
xdes = self.xdes
#invert the transformations from task to world coordinates
if self.baseLinkNo >= 0:
Tb = self.robot.getLink(self.baseLinkNo).getTransform()
xdes = se3.apply(Tb, self.xdes)
x = se3.apply(Tb, x)
glPointSize(10)
glEnable(GL_POINT_SMOOTH)
glBegin(GL_POINTS)
glColor3f(0, 1, 0) #green
glVertex3fv(xdes)
glColor3f(0, 1, 1) #cyan
glVertex3fv(x)
glEnd()
def bin_front_center(self,bin_name):
#you may want to implement this. Returns the world position of the
#front-center of the bin. The visualizer will draw these points if
#'draw_bins' is turned to True.
#TODO:
point = [(apc.bin_bounds[bin_name][0][0]+apc.bin_bounds[bin_name][1][0])/2,(apc.bin_bounds[bin_name][0][1]+apc.bin_bounds[bin_name][1][1])/2,.42]
return se3.apply(self.shelf_xform,point)
def drawGL(self,q): x = self.getSensedValue(q) xdes = self.xdes #invert the transformations from task to world coordinates if self.baseLinkNo >= 0: Tb = self.robot.getLink(self.baseLinkNo).getTransform() xdes = se3.apply(Tb,self.xdes) x = se3.apply(Tb,x) glPointSize(10) glEnable(GL_POINT_SMOOTH) glBegin(GL_POINTS) glColor3f(0,1,0) #green glVertex3fv(xdes) glColor3f(0,1,1) #cyan glVertex3fv(x) glEnd()
def main():
models = [get_reconstructed_model(f,True) for f in model_files]
scene_full = get_raw_depth(depth_file)
errors = []
for i in range(len(objects)):
R,t = icp(models[i]['positions'],scene_full,True)
#apply the ICP results to the model
transformed_points = [se3.apply((R,t),p) for p in models[i]['positions']]
errors.append(matching_error(transformed_points,scene_full))
print "Object",objects[i],"matching error:",errors[-1]
models[i]['positions'] = transformed_points
bestIndex = 0
bestError = min(errors)
for i in range(len(objects)):
if errors[i] == bestError:
bestIndex = i
break
#pick one of the models to visualize
visualization_model = bestIndex
print scene, " scene's best match was the object: ", objects[bestIndex]
opengl_plot = OpenGLPlot(models[visualization_model], scene_full)
opengl_plot.initialize_main_loop()
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 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 order_bin_top(self):
(a, b) = order_bin_bounds
# compute the order bin center in the x-y plane
# return the top as 10% beyond the bin top in z
top = [ (a[0] + b[0])/2, (a[1] + b[1])/2, max([ a[2], b[2] ])*1.1 ]
# transform the order bin into the appropriate location
return se3.apply(order_bin_xform, top)
def bin_front_center(self,bin_name):
#you may want to implement this. Returns the world position of the
#front-center of the bin. The visualizer will draw these points if
#'draw_bins' is turned to True.
boundingBox = apc.bin_bounds[bin_name]
binFrontCenterShelfFrame = [(boundingBox[0][0] + boundingBox[1][0]) / 2, (boundingBox[0][1] + boundingBox[1][1]) / 2, boundingBox[1][2] ]
return se3.apply(self.shelf_xform, binFrontCenterShelfFrame)
def bin_front_center(self,bin_name):
#you may want to implement this. Returns the world position of the
#front-center of the bin. The visualizer will draw these points if
#'draw-bins' is turned to True.
bins_xyz = apc.bin_bounds[bin_name]
Bin = [bins_xyz[0][0] + (bins_xyz[1][0] - bins_xyz[0][0])/2 ,bins_xyz[0][1] + (bins_xyz[1][1] - bins_xyz[0][1])/2, bins_xyz[1][2]]
Bin_World = se3.apply(self.shelf_xform, Bin)
return Bin_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.
bins_xyz = apc.bin_bounds[bin_name]
Bin = [bins_xyz[0][0] + (bins_xyz[1][0] - bins_xyz[0][0])/2 ,bins_xyz[0][1] + (bins_xyz[1][1] - bins_xyz[0][1])/2, bins_xyz[1][2] + 0.2]
Bin_World_Vantage = se3.apply(self.shelf_xform, Bin)
return Bin_World_Vantage
def order_bin_vantage_point(self):
#Compute the local vantage point from the order bin bounds
localVantagePoint = [order_bin_bounds[0][0] + (order_bin_bounds[1][0] - order_bin_bounds[0][0])/2 ,order_bin_bounds[0][1] + (order_bin_bounds[1][1] - order_bin_bounds[0][1])/2, order_bin_bounds[1][2]]
#Apply the xform transform to convert the local vantage point to the world vantage point
worldVantagePoint = se3.apply(order_bin_xform, localVantagePoint)
return worldVantagePoint
def move_to_order_bin(self,object):
"""Sets the robot's configuration so the gripper is over the order bin
If successful, sends the motion to the low-level controller and
returns True.
Otherwise, does not modify the low-level controller and returns False.
"""
collisionTries = 0
left_target = se3.apply(order_bin_xform,[0.0,0.2,order_bin_bounds[1][2]+0.1])
right_target = se3.apply(order_bin_xform,[0.0,-0.2,order_bin_bounds[1][2]+0.1])
qcmd = self.controller.getCommandedConfig()
for i in range(100):
if self.active_limb == 'left':
goal = ik.objective(self.left_gripper_link,local=left_gripper_center_xform[1],world=left_target)
else:
goal = ik.objective(self.right_gripper_link,local=right_gripper_center_xform[1],world=right_target)
#set IK solver initial configuration
if i==0:
self.robot.setConfig(qcmd)
else:
self.randomize_limb_position(self.active_limb)
#solve
if ik.solve(goal,tol=0.1):
#TODO: plan a path
# check that this solution is self collision free
# we call the self collision function of WorldCollider
# and see how many items the generator returns. If it is 0
# then we don't collide (if it is not zero, we don't care what
# they are, we throw the solution away and retry with a different
# initial arm configuration).
collisionTries += 1
collisions = self.collider.robotSelfCollisions(robot = self.robot)
numCols = 0
for cols in collisions:
numCols += 1
self.controller.setMilestone(self.robot.getConfig())
if numCols == 0:
self.controller.setMilestone(self.robot.getConfig())
print "Found self collision free IK solution after " + str(collisionTries) + " tries"
return True
return False
def move_to_order_bin(self,object):
# apply the order_bin_xform to the given point
left_target_point = se3.apply(order_bin_xform, [0.0, 0.2, order_bin_bounds[1][2]+0.1])
right_target_point = se3.apply(order_bin_xform, [0.0, -0.2, order_bin_bounds[1][2]+0.1])
# Setup ik objectives for both arms
left_goal = ik.objective(self.left_gripper_link, local=left_gripper_center_xform[1], world=left_target_point)
right_goal = ik.objective(self.right_gripper_link, local=right_gripper_center_xform[1], world=right_target_point)
qmin, qmax = self.robot.getJointLimits()
for i in range(100):
# initialize to the resting pose
# NOTE: that this (without [:]) doesn't work because the list
# must be copied by value, not reference.
# See python shallow copy vs. deep copy for more details
# ( q = baxter_rest_config ) doesn't work!
q = baxter_rest_config[:]
# use left hand
if self.active_limb == 'left':
# randomly initialize each joint in the arm within joint limits
for jointIndex in self.left_arm_indices:
q[jointIndex] = random.uniform(qmin[jointIndex], qmax[jointIndex])
self.robot.setConfig(q)
# attempt to solve ik objective
if ik.solve(left_goal):
self.config = self.robot.getConfig()
return True
# use right hand
else:
# randomly initialize each joint in the arm within joint limits
for jointIndex in self.right_arm_indices:
q[jointIndex] = random.uniform(qmin[jointIndex], qmax[jointIndex])
self.robot.setConfig(q)
# attempt to solve ik objective
if ik.solve(right_goal):
self.config = self.robot.getConfig()
return True
self.config = self.robot.getConfig()
return False
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 getSensedValue(self, q):
"""Get link x, which is rotation matrix and/or translation
"""
self.robot.setConfig(q)
T = self.link.getTransform()
#check if relative transform task, modify T to local transform
if self.baseLinkNo >= 0:
Tb = self.baseLink.getTransform()
Tbinv = se3.inv(Tb)
T = se3.mul(Tbinv,T)
if self.taskType == 'po':
x = (T[0],se3.apply(T,self.localPosition))
elif self.taskType == 'position':
x = se3.apply(T,self.localPosition)
elif self.taskType == 'orientation':
x = T[0]
else:
raise ValueError("Invalid taskType "+self.taskType)
return x
def getSensedValue(self, q): """Returns CoM position """ self.robot.setConfig(q) com = self.robot.getCom() if self.baseLinkNo >= 0: Tb = self.robot.getLink(self.baseLinkNo).getTransform() Tbinv = se3.inv(Tb) com = se3.apply(Tbinv,com) return com
def display(self):
#draw the world
self.world.drawGL()
lh = Hand('l')
rh = Hand('r')
glDisable(GL_LIGHTING)
glPointSize(5.0)
glDisable(GL_DEPTH_TEST)
glBegin(GL_POINTS)
glColor3f(0,1,0)
glVertex3fv(se3.apply(self.world.robot(0).link(lh.link).getTransform(),lh.localPosition1))
glVertex3fv(se3.apply(self.world.robot(0).link(rh.link).getTransform(),rh.localPosition1))
glColor3f(0,0,1)
glVertex3fv(se3.apply(self.world.robot(0).link(lh.link).getTransform(),lh.localPosition2))
glVertex3fv(se3.apply(self.world.robot(0).link(rh.link).getTransform(),rh.localPosition2))
glColor3f(1,0,0)
glVertex3fv(self.world.rigidObject(0).getTransform()[1])
glEnd()
glEnable(GL_DEPTH_TEST)
def move_to_order_bin(self,object):
"""Sets the robot's configuration so the gripper is over the order bin
If successful, sends the motion to the low-level controller and
returns True.
Otherwise, does not modify the low-level controller and returns False.
"""
qcmd = self.controller.getCommandedConfig()
left_target = se3.apply(order_bin_xform,[0.0,0.2,order_bin_bounds[1][2]+0.1])
right_target = se3.apply(order_bin_xform,[0.0,-0.2,order_bin_bounds[1][2]+0.1])
#retraction goal -- maintain vertical world axis
liftVector = [0,0,0.04]
retractVector = [-0.2,0,0]
self.planner.rebuild_dynamic_objects()
self.robot.setConfig(qcmd)
if self.move_gripper_upright(self.active_limb,liftVector):
self.controller.setMilestone(self.robot.getConfig())
print "Moved to lift goal"
self.waitForMove()
collisionchecker = lambda x:self.planner.check_collision_free_with_object(x,object.info.geometry,self.active_grasp)
if self.move_gripper_upright(self.active_limb,retractVector,collisionchecker):
self.controller.setMilestone(self.robot.getConfig())
print "Moved to retract goal"
self.waitForMove()
retractConfig = self.robot.getConfig()
if self.active_limb == 'left':
placegoal = ik.objective(self.left_gripper_link,local=left_gripper_center_xform[1],world=left_target)
else:
placegoal = ik.objective(self.right_gripper_link,local=right_gripper_center_xform[1],world=right_target)
sortedSolutions = self.get_ik_solutions([placegoal],[self.active_limb],retractConfig,tol=0.1)
if len(sortedSolutions) == 0:
print "Failed to find placement config"
for solution in sortedSolutions:
path = self.planner.plan_transfer(retractConfig,solution[0],self.active_limb,object,self.active_grasp)
if path != None:
self.waitForMove()
self.sendPath(path)
return True
print "Planning failed"
return False
def getSensedValue(self, q):
"""Returns CoM position
"""
self.robot.setConfig(q)
com = self.robot.getCom()
if self.baseLinkNo >= 0:
Tb = self.robot.getLink(self.baseLinkNo).getTransform()
Tbinv = se3.inv(Tb)
com = se3.apply(Tbinv, com)
return com
def getSensedValue(self, q):
"""Get link x, which is rotation matrix and/or translation
"""
self.robot.setConfig(q)
T = self.link.getTransform()
#check if relative transform task, modify T to local transform
if self.baseLinkNo >= 0:
Tb = self.baseLink.getTransform()
Tbinv = se3.inv(Tb)
T = se3.mul(Tbinv, T)
if self.taskType == 'po':
x = (T[0], se3.apply(T, self.localPosition))
elif self.taskType == 'position':
x = se3.apply(T, self.localPosition)
elif self.taskType == 'orientation':
x = T[0]
else:
raise ValueError("Invalid taskType " + self.taskType)
return x
def display(self):
#Put your display handler here
#the current example draws the simulated world in grey and the
#commanded configurations in transparent green
GLSimulationProgram.display(self)
#draw tendons
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
glLineWidth(4.0)
glColor3f(0,1,1)
glBegin(GL_LINES)
for i in range(3):
b1 = self.sim.body(self.handsim.model.robot.link(self.handsim.model.proximal_links[i]))
b2 = self.sim.body(self.handsim.model.robot.link(self.handsim.model.distal_links[i]))
glVertex3f(*se3.apply(b1.getTransform(),self.handsim.tendon1_local))
glVertex3f(*se3.apply(b2.getTransform(),self.handsim.tendon2_local))
glEnd()
glLineWidth(1)
glEnable(GL_DEPTH_TEST)
glEnable(GL_LIGHTING)
def display(self):
#Put your display handler here
#the current example draws the simulated world in grey and the
#commanded configurations in transparent green
self.sim.updateWorld()
self.world.drawGL()
#draw tendons
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
glLineWidth(4.0)
glColor3f(0, 1, 1)
glBegin(GL_LINES)
for i in range(3):
b1 = self.sim.getBody(
self.handsim.model.robot.getLink(
self.handsim.model.proximal_links[i]))
b2 = self.sim.getBody(
self.handsim.model.robot.getLink(
self.handsim.model.distal_links[i]))
glVertex3f(
*se3.apply(b1.getTransform(), self.handsim.tendon1_local))
glVertex3f(
*se3.apply(b2.getTransform(), self.handsim.tendon2_local))
glEnd()
glLineWidth(1)
glEnable(GL_DEPTH_TEST)
glEnable(GL_LIGHTING)
#draw commanded configurations
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, [0, 1, 0, 0.5])
for i in xrange(self.world.numRobots()):
r = self.world.robot(i)
q = self.sim.getController(i).getCommandedConfig()
r.setConfig(q)
r.drawGL(False)
glDisable(GL_BLEND)
def getJacobian(self, q): """Returns axis-weighted CoM Jacobian by averaging mass-weighted Jacobian of each link. """ self.robot.setConfig(q) numLinks = self.robot.numLinks() Jcom = np.array(self.robot.getComJacobian()) #if relative positioning task, subtract out COM jacobian w.r.t. base if self.baseLinkNo >= 0: Tb = self.robot.link(self.baseLinkNo).getTransform() pb = se3.apply(se3.inv(Tb),self.robot.getCom()) Jb = np.array(self.robot.link(self.baseLinkNo).getPositionJacobian(pb)) Jcom -= Jb if self.activeDofs != None: Jcom = select_cols(Jcom,self.activeDofs) return Jcom
def emulate(self, sim):
"""Given a Simulator instance, emulates the sensor.
Result is a CameraColorDetectorOutput structure."""
global objectColors
xscale = math.tan(math.radians(self.fov * 0.5)) * self.w / 2
blobs = []
for i in range(sim.world.numRigidObjects()):
o = sim.world.rigidObject(i)
body = sim.body(o)
Tb = body.getTransform()
plocal = se3.apply(se3.inv(self.Tsensor), Tb[1])
x, y, z = plocal
if z < self.dmin or z > self.dmax:
continue
c = objectColors[i % len(objectColors)]
o.geometry().setCurrentTransform(
so3.mul(so3.inv(self.Tsensor[0]), Tb[0]), [0] * 3)
bmin, bmax = o.geometry().getBB()
err = self.pixelError
dscale = xscale / z
xim = dscale * x + self.w / 2
yim = dscale * y + self.h / 2
xmin = int(xim - dscale * (bmax[0] - bmin[0]) * 0.5 +
random.uniform(-err, err))
ymin = int(yim - dscale * (bmax[1] - bmin[1]) * 0.5 +
random.uniform(-err, err))
xmax = int(xim + dscale * (bmax[0] - bmin[0]) * 0.5 +
random.uniform(-err, err))
ymax = int(yim + dscale * (bmax[1] - bmin[1]) * 0.5 +
random.uniform(-err, err))
if xmin < 0: xmin = 0
if ymin < 0: ymin = 0
if xmax >= self.w: xmax = self.w - 1
if ymax >= self.h: ymax = self.h - 1
if ymax <= ymin: continue
if xmax <= xmin: continue
#print "Actual x,y,z",x,y,z
#print "blob color",c[0:3],"dimensions",xmin,xmax,"x",ymin,ymax
blob = CameraBlob(c[0:3], 0.5 * (xmin + xmax), 0.5 * (ymin + ymax),
xmax - xmin, ymax - ymin)
blobs.append(blob)
return CameraColorDetectorOutput(blobs)
def drawGL(self):
#draw tendons
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
glLineWidth(4.0)
glColor3f(0, 1, 1)
glBegin(GL_LINES)
for i in range(3):
b1 = self.sim.body(
self.model.robot.link(self.model.proximal_links[i]))
b2 = self.sim.body(
self.model.robot.link(self.model.distal_links[i]))
glVertex3f(*se3.apply(b1.getTransform(), self.tendon0_local))
glVertex3f(*se3.apply(b1.getTransform(), self.tendon1_local))
glVertex3f(*se3.apply(b1.getTransform(), self.tendon1_local))
glVertex3f(*se3.apply(b2.getTransform(), self.tendon2_local))
glEnd()
glLineWidth(1)
glColor3f(1, 0.5, 0)
glBegin(GL_LINES)
fscale = 0.01
for i in range(3):
b1 = self.sim.body(
self.model.robot.link(self.model.proximal_links[i]))
b2 = self.sim.body(
self.model.robot.link(self.model.distal_links[i]))
if self.forces[i][0] != None:
p = se3.apply(b1.getTransform(), self.tendon0_local)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p, self.forces[i][0], fscale))
if self.forces[i][1] != None:
p = se3.apply(b1.getTransform(), self.tendon1_local)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p, self.forces[i][1], fscale))
if self.forces[i][2] != None:
p = se3.apply(b2.getTransform(), self.tendon2_local)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p, self.forces[i][2], fscale))
glEnd()
glEnable(GL_DEPTH_TEST)
glEnable(GL_LIGHTING)
unionPoints = []
unionTris = []
for i in range(robot.numLinks()):
T = robot.getLink(i).getTransform()
geom = robot.getLink(i).getGeometry()
t = geom.type()
if t=="TriangleMesh":
trimesh = geom.getTriangleMesh()
tris = []
for index in xrange(0,len(trimesh.indices),3):
tris.append(trimesh.indices[index:index+3])
points = []
for index in xrange(0,len(trimesh.vertices),3):
points.append(trimesh.vertices[index:index+3])
offset = len(unionPoints)
#apply transformation
points = [se3.apply(T,pt) for pt in points]
unionPoints += points
#offset triangle indices by number of existing points
unionTris += [(a+offset,b+offset,c+offset) for (a,b,c) in tris]
print "Writing to",meshfn,"..."
f = open(meshfn,'w')
f.write(str(len(unionPoints))+'\n')
for pt in unionPoints:
f.write('%g %g %g\n'%tuple(pt))
f.write(str(len(unionTris))+'\n')
for tri in unionTris:
f.write('%d %d %d\n'%tuple(tri))
f.close()
def calibrate_xform_camera(camera_link_transforms,
marker_link_transforms,
marker_observations,
marker_ids,
observation_relative_errors=None,
camera_initial_guess=None,
marker_initial_guess=None,
regularizationFactor=0,
maxIters=100,
tolerance=1e-7):
"""Single camera calibration function for a camera and markers on some
set of rigid bodies.
Given body transforms and a list of estimated calibration marker observations
in the camera frame, estimates both the camera transform relative to the
camera link as well as the marker transforms relative to their links.
M: is the set of m markers. By default there is at most one marker per link.
Markers can either be point markers (e.g., a mocap ball), or transform
markers (e.g., an AR tag or checkerboard pattern).
O: is the set of n observations, consisting of a reading (Tc_i,Tm_i,o_i,l_i)
where Tc_i is the camera link's transform, Tm_i is the marker link's
transform, o_i is the reading which consists of either a point or transform
estimate in the camera frame, and l_i is the ID of the marker (by default,
just its link)
Output: a tuple (err,Tc,marker_dict) where err is the norm of the
reconstruction residual, Tc is the estimated camera transform relative to the
camera's link, and marker_dict is a dict mapping each marker id to its
estimated position or transform on the marker's link.
Arguments:
- camera_link: an integer index or a RobotModelLink instance on which
the camera lies.
- calibration_configs: a list of the RobotModel configurations q_1,...,q_n
that generated the marker_observations list.
- marker_observations: a list of estimated positions or transformations
of calibration markers o_1,...,o_n, given in the camera's reference
frame (z forward, x right, y down).
If o_i is a 3-vector, the marker is considered to be a point marker.
If a se3 element (R,t) is given, the marker is considered to be
a transform marker. You may not mix point and transform observations
for a single marker ID.
- marker_ids: a list of marker ID #'s l_1,...,l_n corresponding to
each observation, e.g., the link index on which each marker lies.
- observation_relative_errors: if you have an idea of the magnitude of
each observation error, it can be placed into this list. Must be
a list of n floats, 3-lists (point markers), or 6-lists (transform
markers). By default, errors will be set proportionally to the
observed distance between the camera and marker.
- camera_initial_guess: if not None, an initial guess for the camera transform
- marker_initial_guess: if not None, a dictionary containing initial guesses
for the marker transforms
- regularizationFactor: if nonzero, the optimization penalizes deviation
of the estimated camera transform and marker transforms from zero
proportionally to this factor.
- maxIters: maximum number of iterations for optimization.
- tolerance: optimization convergence tolerance. Stops when the change of
estimates falls below this threshold
"""
if len(camera_link_transforms) != len(marker_ids):
raise ValueError("Must provide the same number of marker IDs as camera transforms")
if len(marker_link_transforms) != len(marker_ids):
raise ValueError("Must provide the same number of marker IDs as marker transforms")
if len(marker_observations) != len(marker_ids):
raise ValueError("Must provide the same number of marker observations as marker transforms")
#get all unique marker ids
marker_id_list = list(set(marker_ids))
#detect marker types
marker_types = dict((v,None) for v in marker_id_list)
for i,(obs,id) in enumerate(zip(marker_observations,marker_ids)):
if len(obs)==3:
if marker_types[id] == 't':
raise ValueError("Provided both point and transform observations for observation #%d, id %s\n"%(i,str(id)))
marker_types[id] = 'p'
elif len(obs)==2 and len(obs[0])==9 and len(obs[1])==3:
if marker_types[id] == 'p':
raise ValueError("Provided both point and transform observations for observation #%d, id %s\n"%(i,str(id)))
marker_types[id] = 't'
else:
raise ValueError("Invalid observation for observation #%d, id %s\n"%(i,str(id)))
n = len(marker_observations)
m = len(marker_id_list)
#get all the observation weights
observation_weights = []
if observation_relative_errors is None:
#default weights: proportional to distance
for obs in marker_observations:
if len(obs) == 3:
observation_weights.append(1.0/vectorops.norm(obs))
else:
observation_weights.append(1.0/vectorops.norm(obs[1]))
observation_weights = [1.0]*len(observation_weights)
else:
if len(observation_relative_errors) != n:
raise ValueError("Invalid length of observation errors")
for err in observation_relative_errors:
if hasattr(err,'__iter__'):
observation_weights.append([1.0/v for v in err])
else:
observation_weights.append(1.0/err)
#initial guesses
if camera_initial_guess == None:
camera_initial_guess = se3.identity()
if any(v == 't' for v in marker_types.itervalues()):
#estimate camera rotation from point estimates because rotations are more prone to initialization failures
point_observations = []
marker_point_rel = []
for i,obs in enumerate(marker_observations):
if len(obs)==2:
point_observations.append(obs[1])
else:
point_observations.append(obs)
marker_point_rel.append(se3.mul(se3.inv(camera_link_transforms[i]),marker_link_transforms[i])[1])
camera_initial_guess = (point_fit_rotation_3d(point_observations,marker_point_rel),[0.0]*3)
print "Estimated camera rotation from points:",camera_initial_guess
if marker_initial_guess == None:
marker_initial_guess = dict((l,(se3.identity() if marker_types[l]=='t' else [0.0]*3)) for l in marker_id_list)
else:
marker_initial_guess = marker_initial_guess.copy()
for l in marker_id_list:
if l not in marker_initial_guess:
marker_initial_guess[l] = (se3.identity() if marker_types[l]=='t' else [0.0]*3)
camera_transform = camera_initial_guess
marker_transforms = marker_initial_guess.copy()
if DO_VISUALIZATION:
rgroup = coordinates.addGroup("calibration")
rgroup.addFrame("camera link",worldCoordinates=camera_link_transforms[-1])
rgroup.addFrame("marker link",worldCoordinates=marker_link_transforms[-1])
rgroup.addFrame("camera estimate",parent="camera link",relativeCoordinates=camera_transform)
rgroup.addFrame("marker estimate",parent="marker link",relativeCoordinates=marker_transforms.values()[0])
for i,obs in enumerate(marker_observations):
rgroup.addFrame("obs"+str(i)+" estimate",parent="camera estimate",relativeCoordinates=obs)
vis.add("coordinates",rgroup)
vis.dialog()
point_observations_only = all(marker_types[marker] == 'p' for marker in marker_id_list)
xform_observations_only = all(marker_types[marker] == 't' for marker in marker_id_list)
if not point_observations_only and not xform_observations_only:
raise NotImplementedError("Can't calibrate camera from mixed point/transform markers yet")
for iters in range(maxIters):
#attempt to minimize the error on the following over all observations i
#camera_link_transform(q_i)*camera_transform*observation_i = marker_link_transform(l_i,q_i)*marker_transform(l_i)
#first, we'll assume the camera transform is fixed and then optimize the marker transforms.
#then, we'll assume the marker transforms are fixed and then optimize the camera transform.
#finally we'll check the error to detect convergence
#1. Estimate marker transforms from current camera transform
new_marker_transforms = dict((l,(TransformStats(zero=marker_initial_guess[l],prior=regularizationFactor) if marker_types[l]=='t' else VectorStats(value,zero=[0.0]*3,prior=RegularizationFactor))) for l in marker_id_list)
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
Trel = se3.mul(se3.inv(Tmlink),se3.mul(Tclink,camera_transform))
if marker_types[marker] == 't':
estimate = se3.mul(Trel,obs)
else:
estimate = se3.apply(Trel,obs)
new_marker_transforms[marker].add(estimate,observation_weights[i])
print "ITERATION",iters
#print " ESTIMATED MARKER TRANSFORMS:",dict((k,v.average) for (k,v) in new_marker_transforms.iteritems())
#2. Estimate camera transform from current marker transforms
new_camera_transform = TransformStats(zero=camera_initial_guess,prior=regularizationFactor)
if point_observations_only:
#TODO: weighted point fitting
relative_points = []
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
pRel = se3.apply(se3.inv(Tclink),se3.apply(Tmlink,new_marker_transforms[marker].average))
relative_points.append(pRel)
new_camera_transform.add(point_fit_xform_3d(marker_observations,relative_points),sum(observation_weights))
else:
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
Trel = se3.mul(se3.inv(Tclink),se3.mul(Tmlink,new_marker_transforms[marker].average))
estimate = se3.mul(Trel,se3.inv(obs))
new_camera_transform.add(estimate,observation_weights[i])
#print " ESTIMATED CAMERA TRANSFORMS:",new_camera_transform.average
#3. compute difference between last and current estimates
diff = 0.0
diff += vectorops.normSquared(se3.error(camera_transform,new_camera_transform.average))
for marker in marker_id_list:
if marker_types[marker]=='t':
diff += vectorops.normSquared(se3.error(marker_transforms[marker],new_marker_transforms[marker].average))
else:
diff += vectorops.distanceSquared(marker_transforms[marker],new_marker_transforms[marker].average)
camera_transform = new_camera_transform.average
for marker in marker_id_list:
marker_transforms[marker] = new_marker_transforms[marker].average
if math.sqrt(diff) < tolerance:
#converged!
print "Converged with diff %g on iteration %d"%(math.sqrt(diff),iters)
break
print " ESTIMATE CHANGE:",math.sqrt(diff)
error = 0.0
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
Tc = se3.mul(Tclink,camera_transform)
if marker_types[marker] == 't':
Tm = se3.mul(Tmlink,marker_transforms[marker])
error += vectorops.normSquared(se3.error(se3.mul(Tc,obs),Tm))
else:
Tm = se3.apply(Tmlink,marker_transforms[marker])
error += vectorops.distanceSquared(se3.apply(Tc,obs),Tm)
print " OBSERVATION ERROR:",math.sqrt(error)
#raw_input()
if DO_VISUALIZATION:
rgroup.setFrameCoordinates("camera estimate",camera_transform)
rgroup.setFrameCoordinates("marker estimate",marker_transforms.values()[0])
for i,obs in enumerate(marker_observations):
rgroup.setFrameCoordinates("obs"+str(i)+" estimate",obs)
vis.add("coordinates",rgroup)
vis.dialog()
if math.sqrt(diff) >= tolerance:
print "Max iters reached"
error = 0.0
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
Tc = se3.mul(Tclink,camera_transform)
if marker_types[marker] == 't':
Tm = se3.mul(Tmlink,marker_transforms[marker])
error += vectorops.normSquared(se3.error(se3.mul(Tc,obs),Tm))
else:
Tm = se3.apply(Tmlink,marker_transforms[marker])
error += vectorops.distanceSquared(se3.apply(Tc,obs),Tm)
return (math.sqrt(error),camera_transform,marker_transforms)
