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)
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
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.