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
Beispiel #2
0
class HandView(object):
    def __init__(self, handColor, objColor, grasps):
        self.setDefaults(handColor, objColor)
        self.setView()
        self.grasps = grasps

    def setDefaults(self, handColor, objColor):
        #handColor and objColor are colors from Colors.py
        #sets any start hardcoded values
        self.fingerOpeningDict = {1: False, 2: False, 3: False}
        self.handMovingForward = {"X": False, "Y": False, "Z": False}
        self.ignoreContact = {"1": True, "2": True, "3": True, "Palm": True}
        self.distJointAngles = {"min": 0, "max": .837}
        self.medJointAngles = {"min": 0, "max": 2.44}
        self.handColor = handColor
        self.objColor = objColor

    def addGroundPlane(self):
        #adds a ground plane to the scene
        self.ground = AddGroundPlane(self.vis)
        self.ground.createGroundPlane(.175)

    def setView(self):
        #creates a scene with a ground plane
        self.vis = Vis()
        self.vis.setCamera(60, 7 * np.pi / 8, 0)
        self.addGroundPlane()

    def addHand(self, grasp):
        #grasp is either "The Two-fer", "The Claw", "The Pinch", "The Cup"
        #adds the hand to the scene and sets the grasp and wrist rotation
        self.Hand = HandVis(self.vis)
        self.Hand.loadHand()
        self.Hand.changeColor(self.handColor)
        self.setGrasp(grasp)
        self.setWristRotation('Z', 0)

    def addObject(self, objParameters):
        #objParameters = {"shape":,"h":,"w":,"e":,"a":}
        #adds an object to the scene based on the object parameters
        #shifts the hand accordingly
        self.Obj = ObjectGenericVis(self.vis)
        self.setHandTranslation(
            'Z', -.075 - (float(objParameters["e"]) / 100) - .02)
        self.setObject(objParameters)
        self.Obj.changeColor(self.objColor)

    def setIgnoreAllContact(self, value):
        #value is either True or False
        #sets all fingers and palm to ignore or not ignore contact with the object
        for link in self.ignoreContact:
            self.ignoreContact[link] = value

    def setIgnoreFingerContact(self, value):
        #value is either True or False
        #sets all fingers to ignore or not ignore contact with the object
        for link in self.ignoreContact:
            if link != "Palm":
                self.ignoreContact[link] = value

    def totalNumberofTranslationIncrements(self, newOffset, increment):
        #newOffset is a float and increment is a float
        #returns the total number of iterations of the increment to reach the newOffset
        return int(math.ceil((newOffset + increment) / increment))

    def translateHand(self, dimension, amount):
        #direction should be either 'X', 'Y', or 'Z' ;;; amount is a float
        #shifts the hand in relation to the object in the direction of the dimension by the amount
        if dimension == 'X':
            self.Hand.localTranslation(np.array([amount, 0, 0]))
        elif dimension == 'Y':
            self.Hand.localTranslation(np.array([0, amount, 0]))
        elif dimension == 'Z':
            self.Hand.localTranslation(np.array([0, 0, amount]))

    def tryTranslateHandByOneIncrement(self, dimension, increment):
        #direction should be either 'X', 'Y', or 'Z' ;;; increment is a float
        #shifts the hand in the direction of dimension by the increment
        #if hand comes into contact with the object and contact is not ignored,
        #hand backs up to where it started and returns False, otherwise returns True
        #if contact is ignored, but the palm is not in contact, it no longer ignores contact
        self.translateHand(dimension, increment)
        pC = self.palmContact()
        if self.ignoreContact["Palm"]:
            self.ignoreContact["Palm"] = pC
            return True
        else:
            if pC:
                self.setHandTranslation(dimension, -increment)
                return False
            else:
                return True

    def setHandTranslation(self, dimension, newOffset):
        #shifts the hand in the direction of dimension by the newOffset
        #if shifting the hand causes a collision between the palm and object,
        #hand is shifted in the opposite direction by one increment so it is no longer touching
        #returns the amount by which the hand was shifted
        direction = newOffset / abs(newOffset)
        increment = .005 * direction
        for i in range(
                1,
                self.totalNumberofTranslationIncrements(newOffset, increment)):
            if not self.tryTranslateHandByOneIncrement(dimension, increment):
                return (i - 1) * increment
        return newOffset

    def setWristRotation(self, direction, rotation):
        #direction should be either 'X', 'Y', or 'Z' ;;; rotation is a float
        #rotates the hand's wrist in the direction specified
        #sets ignore contact for every finger/palm
        if direction == 'X':
            rot = matrixFromAxisAngle([rotation, 0, 0])
        elif direction == 'Y':
            rot = matrixFromAxisAngle([0, rotation, 0])
        elif direction == 'Z':
            rot = matrixFromAxisAngle([0, 0, rotation])
        self.Hand.localRotation(rot)
        self.setIgnoreAllContact(True)

    def setObjRotation(self, direction, rotation):
        #direction should be either 'X', 'Y', or 'Z' ;;; rotation is a float
        #rotates the object in the direction specified
        #sets ignore contact for every finger/palm
        if direction == 'X':
            rot = matrixFromAxisAngle([rotation, 0, 0])
        elif direction == 'Y':
            rot = matrixFromAxisAngle([0, rotation, 0])
        elif direction == 'Z':
            rot = matrixFromAxisAngle([0, 0, rotation])
        self.Obj.localRotation(rot)
        self.setIgnoreAllContact(True)

    def setObject(self, newObj):
        #newObj is a dict {"shape":, "h":, "w":, "e":, "a":}
        #assumes an object has already been added with addObject
        #sets ignore contact for every finger/palm
        if newObj["shape"] == 'cone':
            self.Obj.loadObject(newObj["shape"], int(newObj["h"]),
                                int(newObj["w"]), int(newObj["e"]),
                                int(newObj["a"]))
        else:
            self.Obj.loadObject(newObj["shape"], int(newObj["h"]),
                                int(newObj["w"]), int(newObj["e"]), None)
        self.Obj.changeColor(self.objColor)
        self.setIgnoreAllContact(True)

    def setGrasp(self, newGrasp):
        #newGrasp is one of the keys in self.grasps
        #sets the grasp so that it goes to a certain pregrasp
        #sets ignore contact for every finger/palm
        #returns the new grasp array so the finger sliders can be reset
        self.grasp = self.grasps[newGrasp]
        self.showGrasp()
        self.setIgnoreFingerContact(True)
        return self.grasp

    def getMedJointAngle(self, percentClosed):
        #percentClosed is a float 0<>100
        #returns the grasp array value for a med joint given the percent it is closed
        return ((self.medJointAngles["max"] - self.medJointAngles["min"]) *
                percentClosed) + self.medJointAngles["min"]

    def getDistJointAngle(self, percentClosed):
        #percentClosed is a float 0<>100
        #returns the grasp array value for a the dist joint given the percent it is closed
        return ((self.distJointAngles["max"] - self.distJointAngles["min"]) *
                percentClosed) + self.distJointAngles["min"]

    def fingerOpening(self, finger, percentClosed):
        #finger is either 1, 2, or 3
        #percentClosed is a float 0<>100
        #returns a bool
        if finger == 1:
            return self.getMedJointAngle(percentClosed) < self.grasp[3]
        elif finger == 2:
            return self.getMedJointAngle(percentClosed) < self.grasp[6]
        elif finger == 3:
            return self.getMedJointAngle(percentClosed) < self.grasp[8]

    def anyFingerContact(self):
        #returns whether any finger is in contact with the object
        for contact in self.Hand.getContact(self.Obj.boj).keys():
            if contact.find("finger_") != -1 and contact[-9:] != "prox_link":
                return True
        return False

    def fingerContact(self, finger):
        #finger should be either 1, 2, or 3
        #returns whether the provided finger is in contact with the object
        for contact in self.Hand.getContact(self.Obj.obj).keys():
            if contact.find("finger_" +
                            str(finger)) != -1 and contact[-9:] != "prox_link":
                return True
        return False

    def fullFingerContact(self, finger):
        #finger is either 1, 2, or 3
        #if the tip finger link is in contact, returns 't' (finger is fully in contact)
        #if only the lower finger link is in contact, returns 's' (only lower link in contact)
        #if there is no contact return 'f' (nothing is in contact)
        someContact = False
        for contact in self.Hand.getContact(self.Obj.obj).keys():
            if contact.find("finger_" +
                            str(finger)) != -1 and contact[-9:] == "dist_link":
                return 't'
            elif contact.find("finger_" + str(finger)
                              ) != -1 and contact[-8:] == "med_link":
                someContact = True
        if someContact:
            return 's'
        else:
            return 'f'

    def palmContact(self):
        #returns whether or not the palm is in contact with the object
        for contact in self.Hand.getContact(self.Obj.obj).keys():
            if contact[-9:] == "palm_link":
                return True
        return False

    def fingerChangingDirection(self, finger, percentClosed):
        #finger is either 1, 2, or 3
        #percentClosed is a float 0<>100
        #determines if the finger's motion has changed (was closing, now opening or was opening, now closing)
        return self.fingerOpening(
            finger, percentClosed) != self.fingerOpeningDict[finger]

    def fingerCanBeMovedThisWay(self, finger, percentClosed, contact):
        #finger is either 1, 2, or 3
        #percentClosed is a float 0<>100
        #contact is either 't', 'f', or 's'
        return self.ignoreContact[str(finger)] or self.fingerChangingDirection(
            finger, percentClosed) or contact != 't'

    def setFingerPosition(self, finger, percentClosed, contact):
        #finger is either 1, 2, or 3
        #percentClosed is a float 0<>100
        #contact is either 't'(rue), 'f'(alse), or 's'(some)
        #sets the new grasp values for the finger joints depending on which fingers have contact
        if finger == 1:
            if contact == 'f':
                self.grasp[3] = self.getMedJointAngle(percentClosed)
            self.grasp[4] = self.getDistJointAngle(percentClosed)
        elif finger == 2:
            if contact == 'f':
                self.grasp[6] = self.getMedJointAngle(percentClosed)
            self.grasp[7] = self.getDistJointAngle(percentClosed)
        elif finger == 3:
            if contact == 'f':
                self.grasp[8] = self.getMedJointAngle(percentClosed)
            self.grasp[9] = self.getDistJointAngle(percentClosed)
        self.showGrasp()
        print self.grasp

    def changeFingerPosition(self, finger, percentClosed):
        #finger is either 1, 2, or 3
        #percentClosed is a float 0<>100
        #changes the finger position to the new percentage if possible
        #checks the ignoreContact safeguard can be removed
        #returns whether or not the finger could be moved as suggested
        contact = self.fullFingerContact(finger)
        if self.fingerCanBeMovedThisWay(finger, percentClosed, contact):
            self.fingerOpeningDict[finger] = self.fingerOpening(
                finger, percentClosed)
            self.setFingerPosition(finger, percentClosed, contact)
            if self.ignoreContact[str(finger)]:
                self.ignoreContact[str(finger)] = self.fingerContact(finger)
            return True
        return False

    def getFingerPositionPercent(self, finger):
        #finger is either 1, 2, or 3
        #returns the percent of the finger closed
        if finger == 1:
            return (self.grasp[3] - self.medJointAngles["min"]) / (
                self.medJointAngles["max"] - self.medJointAngles["min"])
        elif finger == 2:
            return (self.grasp[6] - self.medJointAngles["min"]) / (
                self.medJointAngles["max"] - self.medJointAngles["min"])
        elif finger == 3:
            return (self.grasp[8] - self.medJointAngles["min"]) / (
                self.medJointAngles["max"] - self.medJointAngles["min"])

    def showGrasp(self):
        #sets the Hand to the new grasp
        self.Hand.setJointAngles(self.grasp)
Beispiel #3
0
    def Test11(self):  # Description: Generating Images for Interpolated Grasps
        # Oriignal is grey
        # 180 + noise grasp is pink
        # interpolated is violet

        # do STLs with noise after this.

        V = Vis()
        Hand1 = HandVis(V)
        Hand1.loadHand()
        Hand2 = HandVis(V)
        Hand2.loadHand()
        Obj1 = ObjectVis(V)
        Obj1.loadObject(4)
        Obj1.changeColor('green')

        Data = ParseGraspData()
        Data.parseOutputData()
        Data.parseAllTransforms()
        # obj4_cluster13_sub5_grasp2_optimal0_prime.jpg obj4_cluster13_sub5_grasp3_extreme1_target.jpg -- don't have the data.  Tried without class but extreme grasp was too far!
        # grasp1 = Data.findGrasp(objnum = 4, subnum = 5, graspnum = 2, grasptype = 'optimal0', list2check = Data.all_transforms)
        # grasp2 = Data.findGrasp(objnum = 4, subnum = 5, graspnum = 3, grasptype = 'extreme1', list2check = Data.all_transforms)
        # obj4_cluster8_sub4_grasp14_extreme1_prime.jpg obj4_cluster8_sub4_grasp14_optimal0_target.jpg -- don't have the data
        # grasp1 = Data.findGrasp(objnum = 4, subnum = 4, graspnum = 14, grasptype = 'extreme1', list2check = Data.all_transforms)
        # grasp2 = Data.findGrasp(objnum = 4, subnum = 4, graspnum = 14, grasptype = 'optimal0', list2check = Data.all_transforms)
        # obj4_cluster8_sub4_grasp14_extreme1_prime.jpg obj4_cluster8_sub4_grasp9_optimal0_target.jpg -- don't have the data

        case = 4

        #ones that I think look good
        if case == 1:
            grasp1 = Data.findGrasp(objnum=4,
                                    subnum=4,
                                    graspnum=11,
                                    grasptype='optimal0',
                                    list2check=Data.all_transforms)
            grasp2 = Data.findGrasp(objnum=4,
                                    subnum=4,
                                    graspnum=11,
                                    grasptype='optimal0',
                                    list2check=Data.all_transforms)
            filename_base = "obj%s_sub%s_graspnum%s_%s_%s" % (
                4, 4, 11, 'optimal0', 'optimal0')
        elif case == 2:
            grasp1 = Data.findGrasp(objnum=4,
                                    subnum=5,
                                    graspnum=2,
                                    grasptype='optimal0',
                                    list2check=Data.all_transforms)
            grasp2 = Data.findGrasp(objnum=4,
                                    subnum=5,
                                    graspnum=3,
                                    grasptype='extreme1',
                                    list2check=Data.all_transforms)
            filename_base = "obj%s_sub%s_graspnum%s_%s_%s" % (
                4, 5, '2&3', 'optimal0', 'extreme1')
        elif case == 3:
            grasp1 = Data.findGrasp(objnum=4,
                                    subnum=4,
                                    graspnum=14,
                                    grasptype='extreme1',
                                    list2check=Data.all_transforms)
            grasp2 = Data.findGrasp(objnum=4,
                                    subnum=4,
                                    graspnum=14,
                                    grasptype='optimal0',
                                    list2check=Data.all_transforms)
            filename_base = "obj%s_sub%s_graspnum%s_%s_%s" % (
                4, 4, 14, 'extreme1', 'optimal0')
        elif case == 4:
            grasp1 = Data.findGrasp(objnum=4,
                                    subnum=4,
                                    graspnum=14,
                                    grasptype='extreme1',
                                    list2check=Data.all_transforms)
            grasp2 = Data.findGrasp(objnum=4,
                                    subnum=4,
                                    graspnum=9,
                                    grasptype='optimal0',
                                    list2check=Data.all_transforms)
            filename_base = "obj%s_sub%s_graspnum%s_%s_%s" % (
                4, 4, '14&9', 'extreme1', 'optimal0')

        HandT1, ObjT1, Arm_JA1, Hand_JA1 = Data.matricesFromGrasp(grasp1[0])
        HandT2, ObjT2, Arm_JA2, Hand_JA2 = Data.matricesFromGrasp(grasp2[0])
        Hand1.orientHandtoObj(HandT1, ObjT1, Obj1)
        Hand1.setJointAngles(Hand_JA1)
        Hand1.changeColor('greyI')
        Hand2.orientHandtoObj(HandT2, ObjT2, Obj1)
        Hand2.setJointAngles(Hand_JA2)
        Hand2.changeColor('pinkI')
        Hand1.getPalmPoint()
        contact_points1, contact_links1 = Hand1.retractFingers(Obj1)
        contact_points2, contact_links2 = Hand2.retractFingers(Obj1)

        start_axis_angle = np.array([0, 0, 0])
        end_axis_angle = np.array([0, np.pi, 0])
        Hand2.ZSLERP(start_axis_angle,
                     end_axis_angle,
                     1,
                     T_zero=Hand2.obj.GetTransform())
        Hand3 = HandVis(V)
        Hand3.loadHand()
        Hand3.makeEqual(Hand1)
        Hand3.changeColor('blueI')
        alpha = np.linspace(0, 1, 6)[1:-1]

        if case == 1:
            V.setCamera(60, 0, 5 / 4 * np.pi - 0.75)
        elif case == 2:
            V.setCamera(60, np.pi / 4 - 1.5, -np.pi - 1)
        elif case == 3:
            V.setCamera(40, np.pi / 3 + 0.2, -np.pi - 0.5)
        elif case == 4:
            V.setCamera(40, np.pi / 3 - np.pi, -np.pi - 0.75)
            # pdb.set_trace()

        # function to get out dist, az, el of camera from current view
        for a in alpha:
            filename = filename_base + "_alpha%s.png" % (int(10 * a))
            Hand3.ZSLERP(start_axis_angle,
                         end_axis_angle,
                         a,
                         T_zero=Hand1.obj.GetTransform())
            V.clearPoints()
            V.takeImage(filename)
            time.sleep(1)
class TrainingVideo(object):
	def __init__(self):
		self.vis = Vis()
		self.Hand = HandVis(self.vis)
		self.Hand.loadHand()
		self.Obj = ObjectGenericVis(self.vis)
		self.GP = AddGroundPlane(self.vis)
		self.demoObj = list()
		self.frameCount = 0
		self.start_offset = 0

	def recordFrame(self):
		fn = 'Image%04d.png' %self.frameCount
		self.vis.takeImage(fn, delay = False)
		self.frameCount += 1

	# Kadon Engle - last edited 07/14/17
	def fingerRecord(self, oldJA, newJA): # Records a frame for multiple join angles between the starting array (OldJA) amd the ending array (newJA)
		try:
			frame_rate = 20 # An arbitrary base line for the frames in each hand movement
			frames = int(max(abs(newJA - oldJA)) * frame_rate)
			Angles = []
			self.Hand.setJointAngles(oldJA)
			if len(self.Hand.getContactPoints()) > 0:
				self.Hand.setJointAngles(iP[-1])
			for i in range(frames):
				Angles.append(list())
			for i in range(len(oldJA)):
				current = np.linspace(oldJA[i], newJA[i], frames)
				for k in range(frames):
					Angles[k].append(current[k])
			for n, i in enumerate(Angles): #More work is necessary on detecting finger collision to stop the fingers
				self.Hand.setJointAngles(np.array(i))
				if len(self.Hand.getContactPoints()) > 0:
					self.Hand.setJointAngles(Angles[n-1])
					return Angles[n-1]
				self.recordFrame()
			return Angles[n-1]

		except:
			print "Invalid Joint Angle"

	# Kadon Engle - last edited 07/14/17
	def handRecord(self, x, y, z): # Records frames as hand moves in x, y, and/or z direction
		self.T_current = self.Hand.obj.GetTransform()
		T = copy.deepcopy(self.T_current)
		xyz = [x, y, z]
		frames = 20 # Arbitrary value, needs to be changed so that when x, y, or z moves shorter distances, it records less frames. Should be changed so that it records less frames for shorter movements and more frames for longer movements.
		for idx, i in enumerate(xyz):
			if abs(i - self.T_current[idx,3]) > 0.1e-5:
				V = np.linspace(self.T_current[idx,3], i, frames)
				for n in V:
					T[idx,3] = n
					self.Hand.obj.SetTransform(T)
					self.recordFrame()

	def stationaryRecord(self, frames):
		for i in range(int(frames)):
			self.recordFrame()

	def createVideo(self, fn): # creates a video from images recorded in previous section
		# there is a built in openrave function which would be much better, but doesn't work on all hardware
		
		# uses PIL and cv2. opencv is a pain to setup, so may not be worth the hassle
		try:
			if os.path.splitext(fn)[1] != '.avi':
				print("Did not save Video: Invalid File Name")
				return
			initImage = Image.open('Image0001.png')
			height, width, layers = np.array(initImage).shape
			fourcc = cv2.VideoWriter_fourcc(*'XVID')
			video = cv2.VideoWriter(fn, fourcc, 24, (width, height))
			Files = os.listdir(curdir)
			for file in np.sort(Files):
				if os.path.splitext(file)[1] == '.png':
					image = Image.open(file)
					video.write(cv2.cvtColor(numpy.array(image), cv2.COLOR_RGB2BGR))
			video.release()
		except:
			print 'Something went wrong. Maybe you didn\'t record any images'

		# native linux method -- most likely to work
		# subprocess.call(["avconv", "-f", "image2", "-i", "Image%04d.png", "-r", "5", "-s", "800x600", fn+".avi"])

	def removeImages(self): # remove image files that were created
		Files = os.listdir(curdir)
		for file in Files:
			if os.path.splitext(file)[1] == '.png':
				os.remove(file)

	def VLCPlay(self, fn):
		subprocess.call(["vlc", fn])

	def Video1(self): 
		# setup
		self.Obj.loadObject('cube',36,18,3,None)  # TODO: Object with larger extent!
		self.Obj.changeColor('purpleI')
		self.Hand.changeColor('mustard')
		self.GP.createGroundPlane(0.175)
		extent_offset = -3.0/100 # offset by thickness of object
		palm_offset = -0.075 #offset so palm is at 0,0,0
		clearance_offset = -1.0/100 # offset to have clearance between palm and object
		self.start_offset = extent_offset + palm_offset + clearance_offset
		self.Hand.localTranslation(np.array([0, 0, self.start_offset]))
		rot = matrixFromAxisAngle([0,0, np.pi/2])
		self.Hand.localRotation(rot) # rotate hand to put stationary finger on one side of object
		self.T_start = self.Hand.obj.GetTransform() # get starting transform so everything can be done relative to this
		oHand = np.array([0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # position fingers such that hand can move in x, y, and z without hitting object
		cHand = np.array([0, 0, 0.0, 1.3, 0.4, 0.0, 1.3, 0.4, 1.3, 0.4]) # position fingers such that the hand will be closed around the object without hititng it
		self.Hand.setJointAngles(oHand)
		dist_range_min = -0.1
		dist_range_max = 0.1
		frame_rate = 20/0.1 # frames/cm
		# Different arbitrary camera angles for needed viewpoints
		# self.vis.setCamera(60,3*np.pi/4,-np.pi/4-0.1)
		# self.vis.setCamera(60, np.pi, -np.pi/2) # top view
		# self.vis.setCamera(60, -2.25, -np.pi/2.10)
		self.vis.setCamera(60, -2.25, -0.75) # Numbers are completely arbitrary, gives a good view of object and hand.