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