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