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