def Test10(self ): # Description: Add noise and interpolate (take image at end) # do STLs with noise after this. V = Vis() Hand1 = HandVis(V) Hand1.loadHand() Hand2 = HandVis(V) Hand2.loadHand() Obj1 = ObjectVis(V) Obj1.loadObject(4) Data = ParseGraspData() grasps = Data.findGrasp(objnum=4, subnum=4, graspnum=11, list2check=Data.all_transforms) grasp = grasps[1] HandT, ObjT, Arm_JA, Hand_JA = Data.matricesFromGrasp(grasp) Hand1.orientHandtoObj(HandT, ObjT, Obj1) Hand1.setJointAngles(Hand_JA) Hand1.getPalmPoint() contact_points1, contact_links1 = Hand1.retractFingers(Obj1) Contact_JA = Hand1.obj.GetDOFValues() filename_base = "obj%s_sub%s_graspnum%s_grasptype%s" % (4, 4, 11, 'extreme0') if False: T_noise, JA_noise = Hand2.addNoiseToGrasp( Obj1, T_zero=Hand1.obj.GetTransform(), Contact_JA=Contact_JA, TL_n=0.01, R_n=0.1, JA_n=0.1) np.savetxt(filename_base + "_%s.txt" % 'T_noise', T_noise) np.savetxt(filename_base + "_%s.txt" % 'JA_noise', JA_noise) else: JA_noise = np.genfromtxt(filename_base + "_%s.txt" % 'JA_noise') T_noise = np.genfromtxt(filename_base + "_%s.txt" % 'T_noise') Hand2.setJointAngles(JA_noise) Hand2.obj.SetTransform(T_noise) contact_points2, contact_links2 = Hand2.retractFingers(Obj1) alpha = np.linspace(0, 1, 6)[1:-1] start_axis_angle = np.array([0, 0, 0]) end_axis_angle = np.array([0, np.pi, 0]) Hand1.hide() for a in alpha: filename = filename_base + "_alpha%s.png" % (int(10 * a)) Hand2.ZSLERP(start_axis_angle, end_axis_angle, a, T_zero=T_noise) V.clearPoints() V.takeImage(filename) time.sleep(1) pdb.set_trace()
class ShapeImageGenerator(object): def __init__(self): self.model_path = '' self.vis = Vis() self.Hand = HandVis(self.vis) self.Hand.loadHand() self.Hand.localTranslation(np.array([0,0,-0.075])) #offset so palm is at 0,0,0 self.Obj = ObjectGenericVis(self.vis) self.groundPlane = AddGroundPlane(self.vis) self.loadSTLFileList() def loadSTLFileList(self): # get all STL files in directory self.STLFileList = list() directory = curdir + '/../ShapeGenerator/Shapes' for root, dirs, filenames in os.walk(directory): if filenames != []: for fname in filenames: if os.path.splitext(fname)[1] == '.stl': #only want stl files self.STLFileList.append(root + '/' + fname) def valuesFromFileName(self, fn_abs): # gets features of object from file name fn = fn_abs.split('/')[-1] fn_parts = fn.strip('.stl').split('_') shape = fn_parts[0] h = int(fn_parts[1].strip('h')) w = int(fn_parts[2].strip('w')) e = int(fn_parts[3].strip('e')) a = None if len(fn_parts) == 5: # extra feature when there is alpha a = int(fn_parts[4].strip('a')) return shape, h, w, e, a def loadObject(self, objtype, h, w, e, a = None): # loads stl object into scene # only centroid that hasn't been dealt with is the handle. # all centroids shoudl be in the center of the object self.Obj.features['type'] = objtype self.Obj.features['h'] = h self.Obj.features['w'] = w self.Obj.features['e'] = e self.Obj.features['a'] = a result = self.Obj.loadObject(objtype, h, w, e, a) return result def setObjTransparent(self, alpha = 0.5): # change transparency of object self.Obj.changeColor(alpha = alpha) # this can be changed to changeColor def readParameterFile(self, fn): # reads in parameters for creating images from a CSV file params_list = list() with open(fn, 'rb') as file: csvreader = csv.reader(file, delimiter = ',') headers = csvreader.next() for row in csvreader: D = dict() for header,val in zip(headers,row): D[header] = val params_list.append(D) for ip in range(len(params_list)): for k in params_list[ip].keys(): if 'Joint Angles' == k: try: params_list[ip][k] = np.array(params_list[ip][k].split(',')).astype('float')#convert to numpy array except Exception: # for when that entry is blank like when no hand in image # params_list[ip][k] = np.zeros(10) # set to some position so error is not thrown pass elif 'Hand Matrix' == k: # extract array try: params_list[ip][k] = self.stringToArray(params_list[ip][k]) except Exception: # for when that entry is blank like when no hand in image # params_list[ip][k] = np.eye(4) # set to some position so error is not thrown pass elif 'Image Save Name' == k: params_list[ip][k] += '.png' # add extension elif 'Model' == k: params_list[ip][k] += '.stl' # add extension elif 'Model Matrix' == k: # extract array mat_str = params_list[ip][k] mat_num = self.stringToArray(mat_str) params_list[ip][k] = mat_num elif 'Camera Transform' == k: params_list[ip][k] = np.array(params_list[ip][k].split(',')).astype('float')#convert to numpy array elif 'Image Size' == k: i = 1 # should do soemthing here else: print('Unexpected Key: %s' %(k)) self.params_list = params_list return self.params_list def stringToArray(self, mat_str): # Description: convert string to array # had to do this because it is really easy to just copy and paste a matrix into spreadsheet (or save through DictWriter) # The matrix is also much easier to read in the csv file in this form # downside is that when you read it out, the brackets are part of the string. # this can be done in fewer, more efficient steps with regex, but i couldn't figure it out mat_re = re.findall(r'\[.*?\]', mat_str) mat_strip = [t.strip('[]') for t in mat_re] mat_num = np.array([t.split() for t in mat_strip]).astype('float') return mat_num def createImagesFromParametersList(self, shapes = None): # creates images from a list of parameters print("Total: %s" %(len(self.params_list))) counter = 0 for params in self.params_list: counter += 1 if ((shapes == None) or (params['Model'].split('_')[0] in shapes)): # allows only a single set of shapes to be made from list. Mostly during development imageSuccess = self.createImageFromParameters(params) # if not imageSuccess: # pdb.set_trace() print("Current: %s" %counter) def createImageFromParameters(self, params): # creates image from a single set of parameters objLoadSuccess = self.loadObjectFromParameters(params) if objLoadSuccess: self.groundPlane.createGroundPlane(y_height = self.Obj.h/2.0/100) # self.vis.changeBackgroundColor(self.groundPlane.groundPlane.GetLinks()[0].GetGeometries()[0].GetAmbientColor()) self.Obj.changeColor('purpleI') self.Hand.changeColor('yellowI') cam_params = params['Camera Transform'] self.vis.setCamera(cam_params[0], cam_params[1], cam_params[2]) if params['Joint Angles'] is not '' and params['Hand Matrix'] is not '': self.Hand.show() self.Hand.setJointAngles(params['Joint Angles']) self.Hand.obj.SetTransform(params['Hand Matrix']) else: #for images where no hand is shown self.Hand.hide() self.Obj.obj.SetTransform(params['Model Matrix']) if np.sum(self.Obj.obj.GetTransform() - matrixFromAxisAngle([0,0,np.pi/2])) < 1e-4: #if object is rotated 90, use different dimension self.groundPlane.createGroundPlane(y_height = self.Obj.w/2.0/100) else: #offset by height if no rotation -- this is not a great solution when object starts to rotate! self.groundPlane.createGroundPlane(y_height = self.Obj.h/2.0/100) # this should definitely be taken care of when making the spreadsheet pts = self.Hand.getContactPoints() while len(pts) > 0: self.Hand.moveY(-0.001) pts = self.Hand.getContactPoints() # pdb.set_trace() self.vis.takeImage(params['Image Save Name'], delay = True) print("Image Recorded: %s" %params['Image Save Name']) return True else: print("Model Not Found: %s" %params['Model']) return False def loadObjectFromParameters(self, params): # loads objects from a single set of parameters objLoadSuccess = self.Obj.loadObjectFN(self.Obj.stl_path + params['Model']) return objLoadSuccess def loadHandFromParameters(self, params): # sets hand features from a single set of parameters self.Hand.show() self.Hand.setJointAngles(params['Joint Angles']) self.Hand.obj.SetTransform(params['Hand Matrix']) def loadSceneFromParameters(self, params): self.loadObjectFromParameters(params) self.loadHandFromParameters(params) def getParameterFromList(self, list_indx): return self.params_list[list_indx] #get parameters from a location in the list