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 __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 __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 Test3(
self
): # Description: figure out how to apply centroid offset in general
V = Vis()
Obj1 = ObjectVis(V)
Obj1.loadObject(4)
Obj2 = ObjectVis(V)
Obj2.loadObject(4)
Obj3 = ObjectVis(V)
Obj3.loadObject(4)
Hand1 = HandVis(V)
Hand1.loadHand()
Hand2 = HandVis(V)
Hand2.loadHand()
pose = np.random.rand(7)
pose[:4] = pose[:4] / np.linalg.norm(pose[:4])
rot_noTL = rotationMatrixFromQuat(pose[:4])
Obj3.localRotation(rot_noTL)
Obj3.adjustByCentroid()
Obj1.globalTransformation(matrixFromPose(pose))
Obj1.addObjectAxes()
centroid_transform = poseTransformPoints(
pose, -1 * Obj1.objCentroid.reshape(
1, 3)) #effectively a local translation by centroid!
pose_new = np.hstack((pose[:4], centroid_transform.reshape(3, )))
Obj2.globalTransformation(matrixFromPose(pose_new))
Obj2.addObjectAxes()
Obj3.addObjectAxes()
pdb.set_trace()
def Test9(self): # Description: Record image of scene
V = Vis()
Hand1 = HandVis(V)
Hand1.loadHand()
Hand2 = HandVis(V)
Hand2.loadHand()
Obj1 = ObjectVis(V)
Obj1.loadObject(4)
pdb.set_trace()
fname_save = 'test.png'
V.takeImage(fname_save) # test this!
pdb.set_trace()
class ObjChecker(object):
def __init__(self):
self.i = 1
self.vis = Vis()
self.obj = ObjectGenericVis(self.vis)
self.hand = HandVis(self.vis)
self.hand.loadHand()
def loadObject(self, obj_type, h, w, e, a=None):
result = self.obj.loadObject(obj_type, h, w, e, a)
return result
def createObject(self, obj_type, resolution, fnsave, h, w, e, a=None):
pdb.set_trace()
eng = matlab.engine.start_matlab()
if a is not None:
if obj_type == 'vase':
a = a / 100.0
eng.ShapeSTLGenerator(obj_type,
resolution,
fnsave,
h / 100.0,
w / 100.0,
e / 100.0,
a,
nargout=0)
else:
eng.ShapeSTLGenerator(obj_type,
resolution,
fnsave,
h,
w,
e,
nargout=0)
self.obj.loadObject(obj_type, h, w, e, a)
self.obj.changeColor(alpha=0.5)
self.vis.drawPoints([0, 0, 0])
print("Object Created: %s" % result)
def Test1(self): #just visualize a grasp
V = Vis()
Data = ParseGraspData()
Data.parseGraspData()
grasp = Data.val_grasp_data[100]
Obj = ObjectVis(V)
Obj.loadObjectList()
Hand = HandVis(V)
Hand.loadHand()
Obj.loadObject(int(grasp['obj'].strip('obj')))
Hand.globalQuatTranslationMove(grasp['HandRotation'],
grasp['HandPosition'])
Obj.globalQuatTranslationMove(grasp['ObjRotation'],
grasp['ObjPosition'])
V.close()
def Test2(self): #visualize a specific grasp
V = Vis()
Data = ParseGraspData()
Data.parseGraspData()
Data.parseAllTransforms()
grasps = Data.findGrasp(objnum=4,
subnum=4,
graspnum=11,
list2check=Data.all_transforms)
for grasp in grasps:
Obj = ObjectVis(V)
Obj.loadObjectList()
Hand = HandVis(V)
Hand.loadHand()
pdb.set_trace()
Obj.loadObject(grasp['obj'])
Hand.globalTransformation(grasp['HandTransformation'])
Obj.globalTransformation(grasp['ObjTransformation'])
V.close()
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)
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 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.
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
def Test6(self): # Description: Rotate one grasp around center of object
V = Vis()
Hand1 = HandVis(V)
Hand1.loadHand()
Hand2 = HandVis(V)
Hand2.loadHand()
Obj1 = ObjectVis(V)
Obj1.loadObject(4)
Data = ParseGraspData()
Data.parseAllTransforms()
grasps = Data.findGrasp(objnum=4,
subnum=4,
graspnum=11,
list2check=Data.all_transforms)
grasp = grasps[1]
HandT = grasp['HandTransformation']
ObjT = grasp['ObjTransformation']
Arm_JA = grasp['JointAngles'][:7]
Hand_JA = grasp['JointAngles'][7:]
Hand1.orientHandtoObj(HandT, ObjT, Obj1)
Hand1.setJointAngles(Hand_JA)
Hand1.getPalmPoint()
contact_points1, __ = retract_finger.retract_fingers(
V.env, Hand1.obj, Obj1.obj)
V.drawPoints(contact_points1, c='blue')
Hand2.changeColor()
Hand2.orientHandtoObj(HandT, ObjT, Obj1)
T_zero = Hand1.obj.GetTransform()
rot1 = matrixFromAxisAngle([0, 0, np.pi])
rot2 = matrixFromAxisAngle([0, np.pi, 0])
rot3 = matrixFromAxisAngle([np.pi, 0, 0])
# Hand2.obj.SetTransform(np.dot(rot1, T_zero))
Hand2.obj.SetTransform(np.dot(rot2, T_zero))
# Hand2.obj.SetTransform(np.dot(rot3, T_zero))
Hand2.setJointAngles(Hand_JA)
Hand2.getPalmPoint()
contact_points2, __ = retract_finger.retract_fingers(
V.env, Hand2.obj, Obj1.obj)
V.drawPoints(contact_points2, c='green')
pdb.set_trace()
def Test5(self): # Description: Orient Grasp relative to Object
V = Vis()
Hand1 = HandVis(V)
Hand1.loadHand()
Hand2 = HandVis(V)
Hand2.loadHand()
Obj1 = ObjectVis(V)
Obj1.loadObject(4)
Data = ParseGraspData()
Data.parseAllTransforms()
grasps = Data.findGrasp(objnum=4,
subnum=4,
graspnum=11,
list2check=Data.all_transforms)
grasp = grasps[0]
HandT = grasp['HandTransformation']
ObjT = grasp['ObjTransformation']
Arm_JA = grasp['JointAngles'][:7]
Hand_JA = grasp['JointAngles'][7:]
Hand1.orientHandtoObj(HandT, ObjT, Obj1)
Hand1.setJointAngles(Hand_JA)
Hand1.getPalmPoint()
retract_finger.retract_fingers(V.env, Hand1.obj, Obj1.obj)
grasp = grasps[1]
HandT = grasp['HandTransformation']
ObjT = grasp['ObjTransformation']
Arm_JA = grasp['JointAngles'][:7]
Hand_JA = grasp['JointAngles'][7:]
Hand2.orientHandtoObj(HandT, ObjT, Obj1)
Hand2.setJointAngles(Hand_JA)
Hand2.getPalmPoint()
retract_finger.retract_fingers(V.env, Hand2.obj, Obj1.obj)
pdb.set_trace()
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)
def __init__(self):
self.i = 1
self.vis = Vis()
self.obj = ObjectGenericVis(self.vis)
self.hand = HandVis(self.vis)
self.hand.loadHand()
def Test7(
self
): # Description: Interpolate grasp around the center axis of object
V = Vis()
Hand1 = HandVis(V)
Hand1.loadHand()
Hand2 = HandVis(V)
Hand2.loadHand()
Obj1 = ObjectVis(V)
Obj1.loadObject(4)
Data = ParseGraspData()
Data.parseAllTransforms()
grasps = Data.findGrasp(objnum=4,
subnum=4,
graspnum=11,
list2check=Data.all_transforms)
grasp = grasps[1]
HandT = grasp['HandTransformation']
ObjT = grasp['ObjTransformation']
Arm_JA = grasp['JointAngles'][:7]
Hand_JA = grasp['JointAngles'][7:]
Hand1.orientHandtoObj(HandT, ObjT, Obj1)
Hand1.setJointAngles(Hand_JA)
Hand1.getPalmPoint()
contact_points1, __ = retract_finger.retract_fingers(
V.env, Hand1.obj, Obj1.obj)
V.drawPoints(contact_points1, c='blue')
Hand2.changeColor()
T_zero = Hand1.obj.GetTransform()
start_axis_angle = np.array([0, 0, 0])
end_axis_angle = np.array([0, np.pi, 0])
rot2 = matrixFromAxisAngle(end_axis_angle)
Hand2.obj.SetTransform(np.dot(rot2, T_zero))
Hand2.setJointAngles(Hand1.obj.GetDOFValues())
Hand2.getPalmPoint()
contact_points2, __ = retract_finger.retract_fingers(
V.env, Hand2.obj, Obj1.obj)
V.drawPoints(contact_points2, c='green')
Hand3 = HandVis(V)
Hand3.loadHand()
start_quat = quatFromAxisAngle(start_axis_angle)
end_quat = quatFromAxisAngle(end_axis_angle)
alpha = 0.8
interp_axis_angle = (end_axis_angle - start_axis_angle) * alpha
interp_rotation = matrixFromAxisAngle(interp_axis_angle)
Hand3.obj.SetTransform(np.dot(interp_rotation, T_zero))
Hand3.setJointAngles(Hand1.obj.GetDOFValues())
Hand3.getPalmPoint()
contact_points3, __ = retract_finger.retract_fingers(
V.env, Hand3.obj, Obj1.obj)
V.drawPoints(contact_points3, c='red')
pdb.set_trace()
pass
def Test8(self): # Description: Add noise to the grasp
V = Vis()
Hand1 = HandVis(V)
Hand1.loadHand()
Hand2 = HandVis(V)
Hand2.loadHand()
Obj1 = ObjectVis(V)
Obj1.loadObject(4)
Data = ParseGraspData()
Data.parseAllTransforms()
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 = retract_finger.retract_fingers(
V.env, Hand1.obj, Obj1.obj)
Contact_JA = Hand1.obj.GetDOFValues()
V.drawPoints(contact_points1, c='blue')
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)
pdb.set_trace()
return JA_noise, T_noise
def loadAdditionalHand(self):
self.HandList.append(HandVis(self.SIG.vis))
self.HandList[-1].loadHand()
self.HandList[-1].show()
self.HandList[-1].changeColor('yellowI')