def evaluate_pregrasp(self, pre_grasp): GraspitCommander.toggleAllCollisions(False) GraspitCommander.forceRobotDof([pre_grasp.dofs[0], 0, 0, 0]) GraspitCommander.setRobotPose(pre_grasp.pose) GraspitCommander.toggleAllCollisions(True) GraspitCommander.findInitialContact() GraspitCommander.approachToContact(250) GraspitCommander.autoGrasp() result = GraspitCommander.getRobot(0) quality = self.compute_quality() return result, quality
GraspitCommander.setDynamics(False) GraspitCommander.toggleDynamicsController(False) while not GraspitCommander.dynamicAutoGraspComplete(): r = GraspitCommander.getRobot(0) print r.robot.dofs GraspitCommander.setRobotDOFForces([0, 100000000, 100000000, 100000000]) GraspitCommander.stepDynamics(1) r = GraspitCommander.getRobot(0) dofs = list(r.robot.dofs) for i in range(len(dofs)): dofs[i] -= 0.01 GraspitCommander.toggleAllCollisions(False) GraspitCommander.autoOpen(0) GraspitCommander.approachToContact(-10) GraspitCommander.toggleAllCollisions(True) GraspitCommander.approachToContact(10) GraspitCommander.forceRobotDof(dofs) GraspitCommander.autoGrasp(0) graspQuality = GraspitCommander.computeQuality() print "DONE!" print graspQuality.epsilon print graspQuality.volume
def collectData( handType, num ): #Workhorse of this file, handType is the chosen hand, num is the type of shape directory = './' + str(handType) + '_' + str(int( time.time())) + '/' #Directory for saving metrics and labels os.mkdir(directory, 0755) shapes = ['cube', 'ellipse', 'cylinder', 'cone'] shapes = [shapes[num % 4]] eng = matlab.engine.start_matlab() eng.cd(r'/home/irongiant/NearContactStudy/ShapeGenerator/', nargout=0) env = openravepy.Environment() collisionChecker = openravepy.RaveCreateCollisionChecker(env, 'ode') env.SetCollisionChecker(collisionChecker) #env.SetViewer('qtcoin') #Set viewer (optional) robot, init_transform = loadRobot( env, handType ) #load robot into Openrave and get transform of the hand in OpenRAVE iter = 0 #viewer = env.GetViewer() gc = GraspitCommander() GraspitCommander.GRASPIT_NODE_NAME = "/graspit" + str(num + 1) + "/" hand_points = getManuallyLabelledPoints(handType) centerRet, surface_norms = getHandPoints(handType) if handType == 'Barrett': #The 'trianglesTransformed' variable is a hack used to handle the trianglesTransformed = 0 # inconsistencies between the meshes of the hands. I'll explain more in a writeup elif handType == 'pr2_gripper': trianglesTransformed = 2 else: trianglesTransformed = 1 points_in_hand_plane = getGridOnHand( robot, hand_points.keys(), centerRet, surface_norms ) #Generates the points and normals of the grids on the hand points_in_hand_plane = morphPoints( robot, points_in_hand_plane, trianglesTransformed ) #Morphs the points from the grid plane onto the mesh of the hand if handType == 'pr2_gripper': numJoints = 0 else: numJoints = len(robot.GetActiveDOFValues()) T_rotation = numpy.eye(4) T_rotation[0:3, 0:3] = rotation_matrix_from_rpy(0, 0, math.pi / 2) for shape in shapes: #Only iterates over 1 shape, see line 264 parameterShape = generateParameters(shape) for parameters in parameterShape: item, filename = loadObject(env, eng, shape, parameters) STLtoPLY(filename) gc.clearWorld() gc.importRobot(handType) gc.importGraspableBody('/home/irongiant/Grasp-Metrics/Shapes/' + filename + '.ply') robot_pose = getPoseMsg(getRobotPose(numpy.eye(4), handType)) gc.setRobotPose(robot_pose) gc.setGraspableBodyPose( 0, getPoseMsg(numpy.matmul(T_rotation, numpy.eye(4)))) item.SetVisible(1) #Can make item visible or invisible for viewing print filename generateSDF( filename, 0.0025, 70) #Signed distance field needs grid resolution parameters transforms = sample_obj_transforms(env, gc, item, handType) graspit_transform = getRobotPose( transforms, handType) #Get the tarnsform for loading the hand into graspit for transform in transforms: item.SetTransform(transform) #transform[0:3,0:3] = numpy.matmul(transform[0:3,0:3], rotation_matrix_from_rpy(math.pi/2, math.pi, 0)) gc.setGraspableBodyPose( 0, getPoseMsg(numpy.matmul(T_rotation, transform))) print transform print getPoseMsg(transform) generateHandFeatures( env, gc, directory + filename + str(iter), robot, item, filename, transform, points_in_hand_plane, trianglesTransformed, math.pi / 6, 5) #Saves signed distance metric and wedge metric #env.Save(directory + filename + '_' + str(iter) + '.dae') configuration = [0] * (6 + numJoints) noise_std = [0.005, 0.005, 0.005, 0.1, 0.1, 0.1] + [ 0.1 ] * numJoints #Covariance matrix parameters for gaussian noise injection noise_distr = generateGaussianNoise( handType, configuration, noise_std, 100) #Samples noise from multivariate gaussian # volume = epsilon = [] epsilon = [] contacts = [] for noise in noise_distr: noise_induced_transform = numpy.eye(4) noise_induced_transform[0:3, 3] = numpy.array(noise[0:3]) noise_induced_transform[0:3, 0:3] = rotation_matrix_from_rpy( noise[3:6][0], noise[3:6][1], noise[3:6][2]) #if handType != "pr2_gripper": # robot.SetTransform(numpy.matmul(init_transform, noise_induced_transform)) #else: robot.SetTransform( numpy.matmul(noise_induced_transform, init_transform) ) #Inject noise into transform of hand for i in range(6, 6 + numJoints): if noise[i] < 0: noise[i] = 0 if handType != "pr2_gripper": robot.SetDOFValues( noise[6:6 + numJoints], numpy.arange(numJoints) ) #Inject noise into joint angles of hand collExcept = 0 try: robot_pose = getPoseMsg( getRobotPose(noise_induced_transform, handType)) gc.setRobotPose( robot_pose ) #Sets teh same transform set in OpenRAVE into GraspIt! result = gc.computeQuality() #Computes grasp quality except (RobotCollisionException, InvalidRobotPoseException): collExcept = 1 if not env.CheckCollision(robot) and collExcept == 0: if handType == "Barrett": gc.forceRobotDof(noise[6:6 + numJoints]) if handType == 'ShadowHand': gc.approachToContact() gc.autoGrasp() g_contacts = [] try: r = gc.getRobot(0) contact = r.robot.contacts for c in contact: point = c.ps.pose.position pose = c.ps.pose.orientation g_contacts.append([ point.x, point.y, point.z, pose.x, pose.y, pose.z, pose.w ]) result = gc.computeQuality() # volume += [result.volume] epsilon += [ result.epsilon ] #List of grasp qualities as a result of noise contacts.append( g_contacts ) # list of contacts made between hand and object during grasp as a result of noise except InvalidRobotPoseException: # volume += [0] epsilon += [0] if handType != 'ShadowHand': dofs = [0] * numJoints else: dofs = [0] * 17 if handType != 'pr2_gripper': gc.forceRobotDof(dofs) gc.autoOpen() robot.SetTransform(init_transform) robot_pose = getPoseMsg(getRobotPose(numpy.eye(4), handType)) gc.setRobotPose(robot_pose) if handType != "pr2_gripper": robot.SetDOFValues([0] * len(robot.GetActiveDOFValues()), numpy.arange( 0, len(robot.GetActiveDOFValues()))) numpy.savetxt(directory + filename + str(iter) + '_epsi_labels' + '.out', epsilon, delimiter=',', fmt='%s') #Saves the grasp qualities numpy.savetxt(directory + filename + str(iter) + '_cont_labels' + '.out', contacts, delimiter=',', fmt='%s') #Saves the contacts made iter += 1 env.Remove(env.GetBodies()[1])
T = numpy.eye(4) robotPose = Pose() robotPose.position.x = 0 robotPose.position.y = 0 robotPose.position.z = 0 robotPose.orientation.x = q[0] robotPose.orientation.y = q[1] robotPose.orientation.z = q[2] robotPose.orientation.w = q[3] gc.setGraspableBodyPose(0, objPose) gc.setRobotPose(robotPose) dofs = [0, 3, 3, 3] gc.moveDOFToContacts(dofs, [0.01,0.01,0.01,0.01], True) gc.forceRobotDof(dofs) graspQuality = gc.computeQuality() gc.approachToContact() gc.moveDOFToContacts(dofs, [0.01,0.01,0.01,0.01], True) gc.setRobotDOFForces([0, 100000000, 100000000, 100000000]) gc.autoGrasp(0) graspQuality = gc.computeQuality() gc.autoOpen() Openrave Transform: >>> T array([[1. , 0. , 0. , 0. ], [0. , 1. , 0. , 0. ], [0. , 0. , 1. , 0.117],