def __init__(self, world): self.world = world self.current_bin = "A" self.bin_state = { "A": {"done": False, "contents": []}, "B": {"done": False, "contents": []}, "C": {"done": False, "contents": []}, "D": {"done": False, "contents": []}, "E": {"done": False, "contents": []}, "F": {"done": False, "contents": []}, "G": {"done": False, "contents": []}, "H": {"done": False, "contents": []}, "I": {"done": False, "contents": []}, "J": {"done": False, "contents": []}, "K": {"done": False, "contents": []}, "L": {"done": False, "contents": []}, } self.robotModel = world.robot(0) self.state = INITIAL_STATE self.config = self.robotModel.getConfig() self.left_arm_links = [self.robotModel.link(i) for i in LEFT_ARM_LINK_NAMES] self.right_arm_links = [self.robotModel.link(i) for i in RIGHT_ARM_LINK_NAMES] id_to_index = dict([(self.robotModel.link(i).getID(), i) for i in range(self.robotModel.numLinks())]) self.left_arm_indices = [id_to_index[i.getID()] for i in self.left_arm_links] self.right_arm_indices = [id_to_index[i.getID()] for i in self.right_arm_links] self.Tcamera = se3.identity() self.Tvacuum = se3.identity() self.calibratedCameraXform = RIGHT_F200_CAMERA_CALIBRATED_XFORM self.object_com = [0, 0, 0] self.points1 = [] self.points2 = [] self.vacuumPc = Geometry3D() self.vacuumPc.loadFile(VACUUM_PCD_FILE) # Set up serial if REAL_VACUUM: self.serial = serial.Serial() self.serial.port = ARDUINO_SERIAL_PORT self.serial.baudrate = 9600 self.serial.open() if self.serial.isOpen(): self.serial.write("hello") response = self.serial.read(self.serial.inWaiting()) self.turnOffVacuum() # Load JSON with open(PICK_JSON_PATH) as pick_json_file: raw_json_data = json.load(pick_json_file) for k in self.bin_state: self.bin_state[k]["contents"] = raw_json_data["bin_contents"]["bin_" + k] for my_dict in raw_json_data["work_order"]: bin_letter = my_dict["bin"][4] self.bin_state[bin_letter]["target"] = my_dict["item"] self.current_bin = planning.selectBin(self.bin_state)
def __init__(self,xform = se3.identity()): #the transformation of the gripper fingers relative to the object's #local coordinates self.grasp_xform = xform #the command to be sent to the gripper's fingers to close it properly #around this object self.gripper_close_command = [0] #the command to be sent to the gripper's fingers to open it back #after grasping this object self.gripper_open_command = [1] self.pregrasp = se3.identity()
def __init__(self, apc_order, test=False): # if currently a testing run if not test: Master.__init__(self,apc_order) # if currently not a testing run else: self._create_knowledge_base() self._load_apc_order(apc_order) # load the .csv file in /apc/high_level_planning/...csv load_strategy_constants("apc/high_level_planning/Strategy Optimization.csv") # defaultdict is a "high-performance container datatype". Similar to a dictionary # Using "list" as the argument of defaultdict self.order_by_bin = defaultdict(list) # sort the order item by bin # i.e.) [ (binA,[item1, item2]), (binB,[item4,item9]), (...), ... ] for orderitem in self.order: self.order_by_bin[orderitem['bin']].append(orderitem['item']) self.cache = False print "Loading prior point clouds" # load prior point cloud, transforms from databse for items in the knowledgeBase if self.cache: for bin,objects in self.knowledge_base.bin_contents.iteritems(): for o in objects: pcd_file_name = '/tmp/'+o+'_'+bin+".pcd" if os.path.exists(pcd_file_name): self.knowledge_base.object_xforms[o] = se3.identity() self.knowledge_base.object_clouds[o] = pcd.read(open(pcd_file_name))[1]
def __init__(self, world): self.world = world self.current_bin = 'A' self.bin_state = {'A': {'done': False, 'contents': []},'B': {'done': False, 'contents': []}, 'C': {'done': False, 'contents': []}, 'D': {'done': False, 'contents': []}, 'E': {'done': False, 'contents': []}, 'F': {'done': False, 'contents': []}, 'G': {'done': False, 'contents': []}, 'H': {'done': False, 'contents': []}, 'I': {'done': False, 'contents': []}, 'J': {'done': False, 'contents': []}, 'K': {'done': False, 'contents': []}, 'L': {'done': False, 'contents': []}} self.robotModel = world.robot(0) self.state = INITIAL_STATE self.config = self.robotModel.getConfig() self.left_arm_links = [self.robotModel.link(i) for i in LEFT_ARM_LINK_NAMES] self.right_arm_links = [self.robotModel.link(i) for i in RIGHT_ARM_LINK_NAMES] id_to_index = dict([(self.robotModel.link(i).getID(),i) for i in range(self.robotModel.numLinks())]) self.left_arm_indices = [id_to_index[i.getID()] for i in self.left_arm_links] self.right_arm_indices = [id_to_index[i.getID()] for i in self.right_arm_links] self.Tcamera = se3.identity() self.Tvacuum = se3.identity() self.calibratedCameraXform = RIGHT_F200_CAMERA_CALIBRATED_XFORM self.object_com = [0, 0, 0] self.points1 = [] self.points2 = [] self.vacuumPc = Geometry3D() self.vacuumPc.loadFile(VACUUM_PCD_FILE) if REAL_SCALE: self.scale = scale.Scale(); # Set up serial if REAL_VACUUM: self.serial = serial.Serial() self.serial.port = ARDUINO_SERIAL_PORT self.serial.baudrate = 9600 self.serial.open() if self.serial.isOpen(): self.serial.write("hello") response = self.serial.read(self.serial.inWaiting()) self.turnOffVacuum() # Load JSON with open(PICK_JSON_PATH) as pick_json_file: raw_json_data = json.load(pick_json_file) for k in self.bin_state: self.bin_state[k]['contents'] = raw_json_data['bin_contents']['bin_'+k] for my_dict in raw_json_data['work_order']: bin_letter = my_dict['bin'][4] self.bin_state[bin_letter]['target'] = my_dict['item'] self.current_bin = planning.selectBin(self.bin_state)
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 __init__(self, world, xforms=None, title=None): GLRealtimeProgram.__init__(self, title or "World Visualization") self.world = world self.xforms = xforms or [] self.xforms += [se3.identity()] # configure the rendering self.clippingplanes = (0.1, 100) logger.info("visualization ready")
def icp(object,scene): """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. """ #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. return se3.identity()
def __init__(self, clouds, xforms=None, world=None): GLRealtimeProgram.__init__(self, "Cloud Visualization") self.clouds = clouds or [] self.point_lists = [] self.xforms = xforms or [] self.xforms += [se3.identity()] self.world = world self.point_size = 2 # configure the rendering self.clippingplanes = (0.1, 100) logger.info("visualization ready")
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. """ print print 'Trying to grasp ' + object.info.name grasp_transforms = self.knowledge.grasp_xforms(object); #try all of the grasps and stop if one is successful for grasp_t in grasp_transforms: #get the world point goal which is offset from the grasp_t rot_xform = se3.mul(grasp_t,se3.inv(left_gripper_center_xform)) #rot = so3.rotation([0,0,1],math.pi); #rot_t = [rot,[0,0,0]]; world_goal_xyz = rot_xform[1]; 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,R=se3.mul(grasp_t,se3.identity())[0],t=world_goal_xyz) if ik.solve(goal): self.active_limb = 'Left' self.config = self.robot.getConfig() return True cnt = cnt + 1 return False
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.getBody(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(*se3.identity()) 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 __init__(self,zero=se3.identity(),prior=0.0): self.average = zero self.count = prior
def localizeShelf(self): logger.info('localizing shelf') if fake_wait(10, 0.95): return se3.identity() else: return None
def display(self): # you may run auxiliary openGL calls, if you wish to visually debug # self.world.robot(0).setConfig(self.controller.config) self.world.drawGL() # show bin boxes if self.draw_bins: glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,1,0,1]) for b in apc.bin_bounds.values(): draw_oriented_box(ground_truth_shelf_xform,b[0],b[1]) for b in apc.bin_names: c = knowledge.bin_front_center(b) if c: glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,1,0.5,1]) r = 0.01 gldraw.box([c[0]-r,c[1]-r,c[2]-r],[c[0]+r,c[1]+r,c[2]+r]) c = knowledge.bin_vantage_point(b) if c: glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0.5,1,0.5,1]) r = 0.01 gldraw.box([c[0]-r,c[1]-r,c[2]-r],[c[0]+r,c[1]+r,c[2]+r]) # show object state for i in ground_truth_items: if i.xform == None: continue if i.bin_name == 'order_bin': # draw in wireframe glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,0.5,0,1]) draw_oriented_box(i.xform,i.info.bmin,i.info.bmax) continue # if perceived bin has an item, draw it in solid color if knowledge.bin_contents[i.bin_name]!=None and i in knowledge.bin_contents[i.bin_name]: glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,0.5,0,1]) draw_oriented_box(i.xform,i.info.bmin,i.info.bmax) else: # otherwise, draw in wireframe glDisable(GL_LIGHTING) glColor3f(1,0.5,0) draw_oriented_wire_box(i.xform,i.info.bmin,i.info.bmax) glEnable(GL_LIGHTING) if self.draw_grasps: # draw grasps, if available g = knowledge.grasp_xforms(i) if g: for xform in g: gldraw.xform_widget(xform,0.02,0.002) # Show gripper and camera frames if self.draw_gripper_and_camera: left_camera_link = self.world.robot(0).link(left_camera_link_name) right_camera_link = self.world.robot(0).link(right_camera_link_name) left_gripper_link = self.world.robot(0).link(left_gripper_link_name) right_gripper_link = self.world.robot(0).link(right_gripper_link_name) gldraw.xform_widget(left_camera_link.getTransform(),0.1,0.01) gldraw.xform_widget(right_camera_link.getTransform(),0.1,0.01) gldraw.xform_widget(se3.mul(left_gripper_link.getTransform(),left_gripper_center_xform),0.05,0.005, lighting=False, fancy=True) gldraw.xform_widget(se3.mul(right_gripper_link.getTransform(),right_gripper_center_xform),0.05,0.005, lighting=False, fancy=True) # Show world frame and shelf frame gldraw.xform_widget(ground_truth_shelf_xform, 0.1, 0.015, lighting=False, fancy=True) gldraw.xform_widget(se3.identity(), 0.2, 0.037, lighting=False, fancy=True) # Draw order box glDisable(GL_LIGHTING) glColor3f(1,0,0) draw_oriented_wire_box(order_bin_xform,order_bin_bounds[0],order_bin_bounds[1]) glEnable(GL_LIGHTING) return
def __init__(self): self.Tsensor = se3.identity() self.fov = 90 self.w,self.h = 320,240 self.dmin,self.dmax = 0.01,5 self.pixelError = 0.5
def __init__(self, address, port=10000, xform=None): self.raw = RemoteRawCamera(address, port) self.xform = xform or se3.identity()
def __init__(self): self.bin_contents = dict((n,None) for n in apc.bin_names) self.order_bin_contents = [] self.shelf_xform = se3.identity()
#you may want to implement this. Returns a list of world transformations #for the grasps defined for the given object (an ItemInBin instance). #The visualizer will draw these transforms if 'draw_grasps' is turned to True. l = [x.grasp_xform for x in object.info.grasps] r = [se3.mul(object.xform,x) for x in l] #if len(r) > 11: # return [r[11]] #else: # return None return r #a list of actual items -- this is only used for the fake perception module, and your #code should not use these items directly ground_truth_items = [] ground_truth_shelf_xform = se3.identity() def init_ground_truth(): global ground_truth_items ground_truth_items = [apc.ItemInBin(apc.tall_item,'bin_B'), apc.ItemInBin(apc.small_item,'bin_D'), apc.ItemInBin(apc.med_item,'bin_H')] ground_truth_items[0].set_in_bin_xform(ground_truth_shelf_xform,0.2,0.2,0.0) ground_truth_items[1].set_in_bin_xform(ground_truth_shelf_xform,0.5,0.1,math.pi/4) ground_truth_items[2].set_in_bin_xform(ground_truth_shelf_xform,0.6,0.4,math.pi/2) def run_perception_on_bin(knowledge,bin_name): """This is a fake perception module that simply reveals all the items the given bin.""" global ground_truth_items if knowledge.bin_contents[bin_name]==None: #not sensed yet
print "With no arguments, shows some 3D trajectories" #add a point to the visualizer and animate it point = coordinates.addPoint("point") visualization.add("point",point) traj = trajectory.Trajectory() for i in range(10): traj.times.append(i) traj.milestones.append([random.uniform(-1,1),random.uniform(-1,1),random.uniform(-1,1)]) traj2 = trajectory.HermiteTrajectory() traj2.makeSpline(traj) visualization.animate("point",traj2) #add a transform to the visualizer and animate it xform = visualization.add("xform",se3.identity()) traj3 = trajectory.SE3Trajectory() for i in range(10): traj3.times.append(i) rrot = random_rotation() rpoint = [random.uniform(-1,1),random.uniform(-1,1),random.uniform(-1,1)] traj3.milestones.append(rrot+rpoint) visualization.animate("xform",traj3) else: #creates a world and loads all the items on the command line world = WorldModel() for fn in sys.argv[1:]: res = world.readFile(fn) if not res: raise RuntimeError("Unable to load model "+fn)
def display(self): #draw the world self.sim.updateWorld() self.simworld.drawGL() #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]) # only 1 robot in this case, but still use for-loop for generality for i in xrange(self.simworld.numRobots()): r = self.simworld.robot(i) q = self.low_level_controller.getCommandedConfig() r.setConfig(q) r.drawGL(False) glDisable(GL_BLEND) self.drawContactForces() # self.drawRoadmap() # add commands to queue if automatically restarted global automatic if automatic >= 1: automatic += 1 if automatic == 10: self.command_queue.put('l') self.command_queue.put('x') self.command_queue.put('t') automatic = -1 # print self.sim.inContact(i,j) # print self.simworld.getName(i),"-",self.simworld.getName(j), f # t = self.sim.mean # printf("%s - %s: force %g %g %g\n",world.GetName(i).c_str(),world.GetName(j)).c_str(),f.x,f.y,f.z,t.x,t.y,t.z); # print contact.simContactMap(self.sim) # qSim = self.simworld.robot(0).getConfig() # qReal = self.low_level_controller.getSensedConfig() # diff = vectorops.distance(qSim, qReal) # print diff # print self.sim.getActualTorques(0)[1] # print self.sim.getContactForces(1,2) # print self.simworld.robot(0).getID() # print self.simworld.rigidObject(0).getID() # self.sim.enableContactFeedbackAll() # print max(abs(self.sim.getJointForces(self.simworld.robot(0).link(9))[3:6])) # print max(abs(self.sim.getJointForces(self.simworld.robot(0).link(14))[3:6])) # print max(abs(self.sim.getJointForces(self.simworld.robot(0).link(18))[3:6])) # print self.sim.getJointForces(self.simworld.robot(0).link(9)) if self.showFrames: # world frame gldraw.xform_widget(se3.identity(), 0.15, 0.005, lighting=True, fancy=True) # robot frame gldraw.xform_widget(self.simworld.robot(0).link(6).getTransform(), 0.15, 0.005, lighting=True, fancy=True) # object frame gldraw.xform_widget(self.simworld.rigidObject(0).getTransform(), 0.15, 0.005, lighting = True, fancy = True) return
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)
setattr(kb, k, data) #test: visualize result #for v in kb.vantage_point_xforms: # (save,result) = resource.edit(v,kb.vantage_point_xforms[v],'RigidTransform',description="vantage point for bin "+v,world=world) r = PerceptionInterface(kb) kb.shelf_xform = r.localizeShelf() kb.order_bin_xform = r.localizeOrderBin() #kb.object_xforms['crayola_64_ct'] = (so3.rotation([0,0,1], 3.1415/4+3.1415), (1.0, 0.15, 0.70)) # kb.object_xforms['kong_sitting_frog_dog_toy'] = se3.identity() # kb.object_clouds['kong_sitting_frog_dog_toy'] = pcd.read(open('/tmp/kong_sitting_frog_dog_toy_bin_C.pcd'))[1] kb.object_xforms['safety_works_safety_glasses'] = se3.identity() kb.object_clouds['safety_works_safety_glasses'] = pcd.read(open('/tmp/safety_works_safety_glasses_bin_G.pcd'))[1] kb.object_xforms['stanley_66_052'] = se3.identity() kb.object_clouds['stanley_66_052'] = pcd.read(open('/tmp/stanley_66_052_bin_F.pcd'))[1] p = PlanningInterface(kb) p.planMoveToInitialPose() kb.target_bin = 'bin_F' plan = p.planGraspObjectInBin('bin_F','stanley_66_052')
def localizeOrderBin(self): logger.info('localizing order bin') if fake_wait(10, 0.95): return se3.identity() else: return None
def localizeSpecificObject(self, bin, object): logger.info('localizing {} in {}'.format(bin, object)) if fake_wait(10, 0.8): return (se3.identity(), pcd.read(open('/tmp/expo_dry_erase_board_eraser_bin_D.pcd'))[1], dict(zip(self.knowledge_base.bin_contents[bin], [1.0 / len(self.knowledge_base.bin_contents[bin])] * len(self.knowledge_base.bin_contents)))) else: return None
visualization.add("point", point) traj = trajectory.Trajectory() for i in range(10): traj.times.append(i) traj.milestones.append([ random.uniform(-1, 1), random.uniform(-1, 1), random.uniform(-1, 1) ]) traj2 = trajectory.HermiteTrajectory() traj2.makeSpline(traj) visualization.animate("point", traj2) #add a transform to the visualizer and animate it xform = visualization.add("xform", se3.identity()) traj3 = trajectory.SE3Trajectory() for i in range(10): traj3.times.append(i) rrot = random_rotation() rpoint = [ random.uniform(-1, 1), random.uniform(-1, 1), random.uniform(-1, 1) ] traj3.milestones.append(rrot + rpoint) visualization.animate("xform", traj3) else: #creates a world and loads all the items on the command line world = WorldModel()
def display(self): #you may run auxiliary openGL calls, if you wish to visually debug #draw the world self.sim.updateWorld() self.simworld.drawGL() #if you're doing question 1, this will draw the shelf and floor if self.simworld.numTerrains()==0: for i in range(self.planworld.numTerrains()): self.planworld.terrain(i).drawGL() #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]) # only 1 robot in this case, but still use for-loop for generality for i in xrange(self.simworld.numRobots()): r = self.simworld.robot(i) #q = self.sim.controller(i).getCommandedConfig() q = self.low_level_controller.getCommandedConfig() r.setConfig(q) r.drawGL(False) glDisable(GL_BLEND) #show bin boxes if self.draw_bins: glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,1,0,1]) for b in apc.bin_bounds.values(): draw_oriented_box(self.picking_controller.knowledge.shelf_xform,b[0],b[1]) for b in apc.bin_names: c = self.picking_controller.knowledge.bin_front_center(b) if c: glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,1,0.5,1]) r = 0.01 gldraw.box([c[0]-r,c[1]-r,c[2]-r],[c[0]+r,c[1]+r,c[2]+r]) c = self.picking_controller.knowledge.bin_vantage_point(b) if c: glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0.5,1,0.5,1]) r = 0.01 gldraw.box([c[0]-r,c[1]-r,c[2]-r],[c[0]+r,c[1]+r,c[2]+r]) #show object state for i in ground_truth_items: if i.xform == None: continue if i.bin_name == 'order_bin': continue #if perceived, draw in solid color if self.picking_controller.knowledge.bin_contents[i.bin_name]!=None and i in self.picking_controller.knowledge.bin_contents[i.bin_name]: glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,0.5,0,1]) draw_oriented_box(i.xform,i.info.bmin,i.info.bmax) else: #otherwise, draw in wireframe glDisable(GL_LIGHTING) glColor3f(1,0.5,0) draw_oriented_wire_box(i.xform,i.info.bmin,i.info.bmax) glEnable(GL_LIGHTING) if self.draw_grasps: #draw grasps, if available g = self.picking_controller.knowledge.grasp_xforms(i) if g: for grasp,xform in g: gldraw.xform_widget(xform,0.05,0.005,fancy=False) # Draws the object held on gripper obj,limb,grasp = self.picking_controller.held_object,self.picking_controller.active_limb,self.picking_controller.active_grasp if obj != None: if limb == 'left': gripper_xform = self.simworld.robot(0).link(left_gripper_link_name).getTransform() else: gripper_xform = self.simworld.robot(0).link(right_gripper_link_name).getTransform() objxform = se3.mul(gripper_xform,se3.mul(left_gripper_center_xform,se3.inv(grasp.grasp_xform))) glDisable(GL_LIGHTING) glColor3f(1,1,1) draw_oriented_wire_box(objxform,obj.info.bmin,obj.info.bmax) glEnable(GL_LIGHTING) #show gripper and camera frames if self.draw_gripper_and_camera: left_camera_link = self.simworld.robot(0).link(left_camera_link_name) right_camera_link = self.simworld.robot(0).link(right_camera_link_name) left_gripper_link = self.simworld.robot(0).link(left_gripper_link_name) right_gripper_link = self.simworld.robot(0).link(right_gripper_link_name) gldraw.xform_widget(left_camera_link.getTransform(),0.1,0.01,fancy=False) gldraw.xform_widget(right_camera_link.getTransform(),0.1,0.01,fancy=False) gldraw.xform_widget(se3.mul(left_gripper_link.getTransform(),left_gripper_center_xform),0.05,0.005,fancy=False) gldraw.xform_widget(se3.mul(right_gripper_link.getTransform(),right_gripper_center_xform),0.05,0.005,fancy=False) # Show world frame and shelf frame gldraw.xform_widget(ground_truth_shelf_xform, 0.1, 0.015, lighting=False, fancy=True) gldraw.xform_widget(se3.identity(), 0.2, 0.037, lighting=False, fancy=True) #draw order box glDisable(GL_LIGHTING) glColor3f(1,0,0) draw_oriented_wire_box(order_bin_xform,order_bin_bounds[0],order_bin_bounds[1]) glEnable(GL_LIGHTING) return
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) visualization.add("coordinates",rgroup) visualization.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) visualization.add("coordinates",rgroup) visualization.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)
# -.controller.task # # reads in from system state # - .command.qcmd # - .controller.traj_q_end # - .robot.endEffectors.0.dest_xform # - .robot.endEffectors.1.dest_xform modes = ['normal', 'scaling', 'gripper'] defaultDeviceState = { 'mode': 'normal', 'buttonDown': False, 'moveStartDeviceTransform': None, 'moveStartWidgetTransform': None, 'widgetTransform': se3.identity(), 'rotationScale': 1, 'positionScale': 3 } deviceState = [] #device +z is gripper -z #device +x is gripper -y #device +y is gripper -x deviceToWidgetTransform = ([0, -1, 0, -1, 0, 0, 0, 0, -1], [0, 0, 0]) deviceToBaxterBaseTransform = ([0, -1, 0, 0, 0, 1, -1, 0, 0], [0, 0, 0]) endEffectorIndices = [25, 45] #widget coordinates in local frame of end effector endEffectorLocalTransforms = [(so3.identity(), (0, 0, 0.08)),
def _localizeRollodexCups(self, world, color, clean_cloud_aligned, depth_uv, clean_mask, object_mask): # copy the uv map because it is modified below # also flip x/y since numpy is row/column uv = depth_uv[:,:,::-1].copy() # scale the uv coordinates by the source image size for i in range(2): uv[:,:,i] = numpy.clip(uv[:,:,i] * color.shape[i], 0, color.shape[i]-1) # round to the nearest pixel uv = uv.round().astype(numpy.int) # filter the uv mapping using the bounding box coordinates ((bb_top, bb_left), (bb_bottom, bb_right)) = match_rollodex_template(color) from scipy.misc import imsave imsave('/tmp/cups.png', color[bb_top:bb_bottom, bb_left:bb_right]) logger.debug('rollodex cups template match bounding box: {}'.format(((bb_top, bb_left), (bb_bottom, bb_right)))) cups_mask = reduce(numpy.bitwise_and, [ uv[:,:,0] > bb_top, uv[:,:,0] < bb_bottom, uv[:,:,1] > bb_left, uv[:,:,1] < bb_right ]) #imsave('/tmp/cups_uv.png', uv) imsave('/tmp/cups_mask.png', cups_mask) valid_depth_pixels = numpy.count_nonzero(cups_mask) if valid_depth_pixels == 0: logger.error('rollodex cups template has no valid depth pixels') return None logger.debug('rollodex cups template has {} valid depth pixels'.format(valid_depth_pixels)) # get the point cloud corresponding to the cups bounding box mask = object_mask & cups_mask cups_cloud = clean_cloud_aligned[mask[clean_mask]] cups_front = cups_cloud[:,0].min() cups_mean = cups_cloud.mean(axis=0) known_cups_width = 0.10 - 0.02 known_cups_height = 0.13 - 0.02 # generate a fake point cloud for the cups n = 32 fake_cloud = numpy.dstack(( cups_front * numpy.ones((n, n)), numpy.dstack(numpy.meshgrid( known_cups_width * numpy.linspace(-0.5, 0.5, n) + cups_mean[1], known_cups_height * numpy.linspace(-0.5, 0.5, n) + cups_mean[2], indexing='ij' )) )) xform = se3.identity() object_cloud = fake_cloud.reshape((-1, 3)) (target_bin, target_object) = (self.knowledge_base.target_bin, self.knowledge_base.target_object) bin_contents = self.knowledge_base.bin_contents[target_bin] if target_bin in self.knowledge_base.bin_contents else [] confusion_row = dict([ (obj, [0, 1][obj == 'rollodex_mesh_collection_jumbo_pencil_cup']) for obj in bin_contents ]) logger.info('confusion row for {}: {}'.format(target_object, confusion_row)) object_cloud_color = [ (p + [ [0, 0, 0] ]) for p in object_cloud.tolist() ] #debug_cloud([ object_cloud_color ], [ (so3.identity(), cups_mean) ], world=world) pcd.write(object_cloud_color, open('/tmp/{}_{}.pcd'.format(target_object, target_bin), 'w')) return (xform, object_cloud_color, confusion_row)
), try: parts = str(raw_input()).split(' ') except (EOFError, KeyboardInterrupt): print break try: 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] = (
def localizeSpecificObject(self, bin, object): SHELF_DIMS_PATH = os.path.join('kb', 'shelf_dims.json') SHELF_CLOUD_PATH = os.path.join('perception', 'segmentation', 'models', 'shelf_thin_10000.npy') # acquire the camera image address = { 'left': '192.168.0.103', 'right': '192.168.0.104' }[self.knowledge_base.active_limb] #address = { 'left': '192.168.0.103', 'right': '192.168.0.103' }[self.knowledge_base.active_limb] camera = RemoteCamera(address, xform=self.camera_xform) camera.read() camera.close() # load the shelf subtraction data bin_bounds = json.load(open(SHELF_DIMS_PATH))[bin] # load and transform the shelf point cloud shelf_cloud = numpy.load(open(SHELF_CLOUD_PATH)) shelf_cloud = shelf_cloud.dot(numpy.array(self.knowledge_base.shelf_xform[0]).reshape((3,3))) + self.knowledge_base.shelf_xform[1] # load and transform the shelf model world = WorldModel() shelf = world.loadRigidObject(os.path.join('klampt_models', 'north_shelf', 'shelf_with_bins.obj')) shelf.setTransform(*self.knowledge_base.shelf_xform) # clean up the point cloud (clean_mask, clean_cloud) = camera.clean() _, clean_cloud_aligned, _, clean_object_mask = shelf_subtract(shelf_cloud, clean_cloud, shelf, bin_bounds, downsample=1000) object_mask = numpy.zeros(camera.cloud.shape[:2], dtype=numpy.bool) object_mask[clean_mask] = clean_object_mask #clean_cloud_aligned_color = map(lambda x: x[0] + [ x[1] ], zip(clean_cloud_aligned[clean_object_mask].tolist(), camera.colorize()[clean_mask][clean_object_mask].tolist())) #debug_cloud([ clean_cloud_aligned_color ], [ self.base_xform, self.camera_xform ]) #debug_cloud([ shelf_cloud, clean_cloud, clean_cloud_aligned ]) #pcd.write(clean_cloud_aligned_color, open('/tmp/{}_{}.pcd'.format(self.knowledge_base.target_object, self.knowledge_base.target_bin), 'w')) (target_bin, target_object) = (self.knowledge_base.target_bin, self.knowledge_base.target_object) bin_contents = self.knowledge_base.bin_contents[target_bin] if target_bin in self.knowledge_base.bin_contents else [] # perform special handling if target_object == 'rollodex_mesh_collection_jumbo_pencil_cup': logger.info('running specialized rollodex cups detection') return self._localizeRollodexCups(world, camera.color, clean_cloud_aligned, camera.depth_uv, clean_mask, object_mask) # dilate the object mask # from scipy.misc import imsave from scipy.ndimage.morphology import binary_dilation object_mask_dilated = binary_dilation(object_mask, iterations=2) # imsave('/tmp/test.png', object_mask) # label connected components (object_labeled, label_count) = distance_label(camera.cloud, object_mask_dilated, threshold=0.01) logger.info('found {} connected components'.format(label_count)) #KH: added to help debugging without changing order bin contents if object not in bin_contents: logger.warning("Warning, trying to detect object "+object+" not in bin contents, adding a temporary entry") bin_contents = bin_contents + [object] bin_contents_with_shelf = bin_contents + [ 'shelf' ] if not label_count: logger.warning('found no connected components') mask = object_mask bin_contents = self.knowledge_base.bin_contents[self.knowledge_base.target_bin] confusion_row = dict(zip(bin_contents, [0]*len(bin_contents))) else: object_blobs = sorted([ object_labeled == (l+1) for l in range(label_count) ], key=lambda b: -numpy.count_nonzero(b)) #for (i, blob) in enumerate(object_blobs): # logger.debug('blob {}: {} points'.format(i, numpy.count_nonzero(blob))) # filter out blobs with fewer than 1000 points min_point_count = 1000 object_blobs = filter(lambda blob: numpy.count_nonzero(blob[clean_mask]) >= min_point_count, object_blobs) #debug_cloud([ clean_cloud_aligned[(object_mask & blob_mask)[clean_mask]] for blob_mask in object_blobs ], [ self.base_xform, self.camera_xform ], wait=False) known_object_count = len(bin_contents) if known_object_count == 0: # return all large blobs n = len(object_blobs) # take only the largest n blobs blob_mask = reduce(numpy.bitwise_or, object_blobs[:n]) mask = object_mask & blob_mask logger.warning('no objects in bin... -> returning all blobs') else: # load the matching statistics match_stats = json.load(open(os.path.join('perception', 'segmentation', 'match_stats.json'))) # load the object hue histograms known_histograms = {} for obj in bin_contents_with_shelf: #array = numpy.load(os.path.join('perception', 'hue_array', '{}.npz'.format(obj)))['arr_0'] #known_histograms[obj] = numpy.histogram(array, bins=range(0,181), density=True)[0].reshape(-1,1).astype(numpy.float32) known_histograms[obj] = numpy.load(os.path.join('perception', 'uv_hist2', '{}.npz'.format(obj)))['arr_0'] # compute the color validity mask color_valid_mask = ~invalid_mask(camera.depth_uv) #KH addition 5/23: test to see whether blobs should be broken up if len(object_blobs)==1 and len(known_histograms) > 2: logger.info("Trying KMeans segmentation to break up large blob") blob_mask = reduce(numpy.bitwise_or, object_blobs) mask = object_mask & blob_mask & color_valid_mask labels,k,score = xyzrgb_segment_kmeans(camera.cloud[mask],camera.colorize()[mask],known_histograms.values()) labels = numpy.array(labels) object_blobs = [] for i in xrange(len(known_histograms)): blank = numpy.zeros_like(object_mask,dtype=numpy.bool) blank[mask] = (labels==i) object_blobs.append(blank) # score each blob for each object in the bin blob_scores = [] blob_accepts = [] blob_rejects = [] matched_blobs = {} confusion_matrix = [] for (i, blob) in enumerate(object_blobs): # compute the blob histogram blob_uv = [ rgb2yuv(*rgb)[1:3] for rgb in camera.colorize()[object_mask & blob & color_valid_mask] ] blob_histogram = make_uv_hist(blob_uv) # compare the blob to each possible object scores = dict([ (obj, 2 * numpy.minimum(blob_histogram, histogram).sum() - numpy.maximum(blob_histogram, histogram).sum()) for (obj, histogram) in known_histograms.items() ]) logger.debug('blob {}:'.format(i)) blob_scores.append(scores) logger.debug(' scores: {}'.format([ '{}={}'.format(*x) for x in scores.items() ])) # apply the cutoff to each score and associate with positive confidence accepts = dict([ (obj, match_stats[obj]['ppv']) for obj in bin_contents_with_shelf if scores[obj] >= match_stats[obj]['cutoff'] ]) blob_accepts.append(accepts) logger.debug(' accepts: {}'.format([ '{}={}'.format(*x) for x in accepts.items() ])) # apply the cutoff to each score and associate with negative confidence rejects = dict([ (obj, match_stats[obj]['npv']) for obj in bin_contents_with_shelf if scores[obj] < match_stats[obj]['cutoff'] ]) blob_rejects.append(rejects) logger.debug(' rejects: {}'.format([ '{}={}'.format(*x) for x in rejects.items() ])) # populate the confusion matrix shelf_probability = 0.1 S = lambda o: match_stats[o]['specificity'] iS = lambda o: 1 - S(o) total_probability = sum(map(S, accepts)) + sum(map(iS, rejects)) + shelf_probability confusion_matrix.append([ [ iS(o), S(o) ][ o in accepts ] / total_probability for o in bin_contents ]) # resolve multiple assignments if len(accepts) > 1: # choose the object with the highest matched confidence best_match_object = sorted(accepts.items(), key=lambda a: -a[1])[0][0] # remove all other objects for (obj, confidence) in accepts.items(): if obj != best_match_object: del accepts[obj] logger.warn('blob {} multiple accept ignored: {}={}'.format(i, obj, confidence)) if len(accepts) == 1: # a single match so record it matched_blobs.setdefault(accepts.keys()[0], []).append(i) logger.info('blob {} assigned to {} by single match'.format(i, accepts.keys()[0])) confusion_matrix = numpy.array(confusion_matrix) logger.debug('confusion matrix:\n{}'.format(numpy.array(confusion_matrix))) # assign blobs by least threshold difference until all objects are assigned (excluding shelf) or all blobs are assigned while len([ k for k in matched_blobs.keys() if k != 'shelf']) < len(bin_contents) and len(sum(matched_blobs.values(), [])) < len(object_blobs): best_match_objects = [] for (i, scores) in enumerate(blob_scores): # only consider unmatched blobs if i not in sum(matched_blobs.values(), []): threshold_differences = [ [obj, match_stats[obj]['cutoff'] - score] for (obj, score) in scores.items() ] best_match_objects.append([ i ] + sorted(threshold_differences, key=lambda d: d[1])[0]) # choose the least threshold difference across all blobs and objects best_blob, best_object, _ = sorted(best_match_objects, key=lambda b: b[2])[0] matched_blobs.setdefault(best_object, []).append(best_blob) logger.warn('blob {} assigned to {} by least threshold difference'.format(best_blob, best_object)) # report unmatched blobs and objects (excluding shelf) for obj in bin_contents: if obj not in matched_blobs: logger.warn('no blobs matched {}'.format(obj)) for i in range(len(object_blobs)): if i not in sum(matched_blobs.values(), []): logger.warn('blob {} is unassigned'.format(i)) for obj in bin_contents_with_shelf: if obj in matched_blobs: print obj mask = reduce(numpy.bitwise_or, [ object_blobs[i] for i in matched_blobs[obj] ], numpy.zeros_like(object_mask)) object_cloud = clean_cloud_aligned[mask[clean_mask]] xform = (so3.identity(), object_cloud.mean(axis=0).tolist()) object_cloud_color = map(lambda x: x[0] + [ x[1] ], zip(object_cloud.tolist(), camera.colorize()[mask].tolist())) #debug_cloud([ object_cloud_color ], [ self.base_xform, self.camera_xform, xform ]) # check that the target object was found if target_object not in matched_blobs: logger.error('no blobs assigned to target object') return None # return blob for the selected object mask = reduce(numpy.bitwise_or, [ object_blobs[i] for i in matched_blobs[target_object] ], numpy.zeros_like(object_mask)) # return the confusion matrix rows for the selected object confusion_rows = confusion_matrix[matched_blobs[target_object], :] confusion_row = dict([ (obj, confusion_rows[:, i].mean()) for (i, obj) in enumerate(bin_contents) ]) logger.info('confusion row for {}: {}'.format(target_object, confusion_row)) object_cloud = clean_cloud_aligned[mask[clean_mask]] #xform = (so3.identity(), object_cloud.mean(axis=0).tolist()) xform = se3.identity() object_cloud_color = map(lambda x: x[0] + [ x[1] ], zip(object_cloud.tolist(), camera.colorize()[mask].tolist())) #debug_cloud([ object_cloud_color ], [ self.base_xform, self.camera_xform, xform ]) # downsample the cloud to about 1000 points ratio = int(len(object_cloud) / 1000) if ratio > 1: object_cloud_color = object_cloud_color[::ratio] pcd.write(object_cloud_color, open('/tmp/{}_{}.pcd'.format(self.knowledge_base.target_object, self.knowledge_base.target_bin), 'w')) return (xform, object_cloud_color, confusion_row)
grippermodules = {'reflex':reflex_col, 'rethink_electric_gripper':rethink_electric_gripper, } grippermodule = grippermodules[grippername] world = WorldModel() world.readFile(os.path.join('../klampt_models/',grippermodule.klampt_model_name)) robot = world.robot(0) #load the items and spawn one in the world apc.load_item(objectname,gripper=grippername) item = apc.item_info[objectname] item.geometry = apc.load_item_geometry(item) itemposed = apc.ItemInBin(item,'blah') itemposed.xform = se3.identity() apc.spawn_item_in_world(itemposed,world) #set the directory print "Resouce directory is",os.path.join('../resources/',os.path.splitext(grippermodule.klampt_model_name)[0]) resource.setDirectory(os.path.join('../resources/',os.path.splitext(grippermodule.klampt_model_name)[0])) #edit the default configurations default_open_config = resource.get('default_open.config', description='Default hand open configuration', doedit='auto', world=world) default_closed_config = resource.get('default_closed.config', description='Default hand closed configuration', doedit='auto', world=world)
def icp(object,scene,reuse=True): """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. """ extract_resolution = 0.01 (obj, obj_connectivity) = clean_cloud(numpy.array(object), 0.01) print 'sampled object', 100*extract_resolution, 'cm:', len(object), '->', len(obj) # get the object properties obj_dims = numpy.amax(object, axis=0) - numpy.amin(object, axis=0) obj_volume = numpy.count_nonzero(obj_connectivity) * extract_resolution**3 print 'object', '%.1fx%.1fx%.1f' % tuple(100*obj_dims), 'cm', '(%.1f' % (100**3*obj_volume,), 'cm^3)' # this caching code is to make detect_3d run much faster # otherwise it would need to run connected components each time which is slow global cached if reuse and cached: (scene, scene_tree, grids, scene_connectivity, resolution) = cached else: # fast point cloud lookups print 'generate scene kdtree' scene = numpy.array(scene) scene_tree = scipy.spatial.KDTree(scene[::]) # run connected components (grids, scene_connectivity, resolution) = multiscale_connected_components(scene, scene_tree, 0.10, [1, 4, 8, 16]) # cache the results cached = (scene, scene_tree, grids, scene_connectivity, resolution) inflated_resolution = resolution*2**0.5 # thin the connectivity grids # this is needed to separate the kygen_eggs and kong_duck from the table # unfortunately, it also destroys the highlighters... for i in range(6): scene_connectivity = thin_connectivity(scene_connectivity) labeled, label_count = scipy.ndimage.label(scene_connectivity) candidates = [] # compute candidate coarse bounds index_bounds = scipy.ndimage.find_objects(labeled) for label in range(label_count): ib = index_bounds[label] lbounds = numpy.array([ grids[i][ib[0].start, ib[1].start, ib[2].start] for i in range(3) ]) - inflated_resolution ubounds = numpy.array([ grids[i][ib[0].stop-1, ib[1].stop-1, ib[2].stop-1] for i in range(3) ]) + inflated_resolution # compute rough dimensions and volume dim = ubounds - lbounds rough_volume = numpy.count_nonzero(labeled == label) * resolution**3 candidates.append({ 'label': label+1, 'rough_bounds': numpy.vstack((lbounds, ubounds)), 'rough_volume': rough_volume }) print 'found candidate', '~%.1fx%.1fx%.1f' % tuple(100*dim), 'cm', '(~%.1f' % (100**3*rough_volume,), 'cm^3)' if len(candidates) == 0: print 'no candidates found!' return se3.identity, float('inf') # sort candidates such that the one with estimated volume closest to the target object is matched first candidates.sort(key=lambda c: abs(c['rough_volume'] - obj_volume)) # extra candidate point clouds and get finer bounds for candidate in candidates: # reject large candidates before extraction to save time if candidate['rough_volume'] > obj_volume * 10: print 'candidate too large for good match -- skipping' continue (candidate['points'], candidate['connectivity']) = extract_scene_candidate(scene_tree, grids, labeled == candidate['label'], resolution, extract_resolution) ubounds = numpy.amax(candidate['points'], axis=0) lbounds = numpy.amin(candidate['points'], axis=0) # compute fine dimensions and volume dim = ubounds - lbounds fine_volume = numpy.count_nonzero(candidate['connectivity']) * extract_resolution**3 candidate['fine_bounds'] = numpy.vstack((lbounds, ubounds)) candidate['fine_volume'] = fine_volume print 'extracted candidate', 100*extract_resolution, 'cm:', '%.1fx%.1fx%.1f' % tuple(100*dim), 'cm', '(%.1f' % (100**3*fine_volume,), 'cm^3)' if fine_volume < obj_volume / 10: print 'candidate too small for good match -- skipping' continue # time to attempt a match to this candidate R, t, metric = match_candidate(obj, scipy.spatial.KDTree(candidate['points']), 100) # save results for later candidate.update({ 'transform': (R.T.flat[:], t[0]), # needed output format 'metric': metric }) if metric < 1: print 'good enough fit found:', metric return candidate['transform'], metric #[ c.get('fine_bounds', c['rough_bounds']) for c in candidates ] else: print 'candidate is poor fit -- trying another' # no good candidates found so just pick the one with the best metric candidates.sort(key=lambda c: c.get('metric', float('inf'))) best_candidate = candidates[0] print 'best fit:', best_candidate.get('metric', float('inf')) return best_candidate.get('transform', se3.identity()), best_candidate.get('metric', float('inf'))#, [ c.get('fine_bounds', c['rough_bounds']) for c in candidates ]