added two other wrench quality measures: volume and isotropy

hw1_grasp.py

@@ -150,10 +150,19 @@ def eval_grasp(self, grasp):
 
           G[:, i] = wrench
 
-        # Use SVD to compute minimum score
+        k1 = 500
+        k2 = 100
+        k3 = 500
+
         U, S, V = np.linalg.svd(G)
-        score = S[-1]
-        return score 
+        sigmaMin = abs(S[-1])
+        sigmaMax = abs(S[0])
+        volumeG = np.linalg.det(np.dot(G, np.transpose(G))) ** 0.5
+        isotropy = abs(float(sigmaMin) / sigmaMax)
+        print sigmaMin, volumeG, isotropy
+        score = k1 * sigmaMin + k2 * volumeG + k3 * isotropy
+        return score
+
 
       except openravepy.planning_error,e:
         #you get here if there is a failure in planning
@@ -240,13 +249,4 @@ def show_grasp(self, grasp, delay=1.5):
     #print 'Showing grasp ', i
     #robo.show_grasp(robo.grasps_ordered[i], delay=delay)
     print 'Showing noisy grasp ', i
-    robo.show_grasp(robo.grasps_ordered_noisy[i], delay=delay)
-
-
-  #import IPython
-  #IPython.embed()
-  #print "Showing grasps..."
-  #for grasp in robo.grasps_ordered:
-  # robo.show_grasp(grasp)
-  #time.sleep(10000) #to keep the openrave window open
-
+    robo.show_grasp(robo.grasps_ordered_noisy[i], delay=delay)

hw1_grasp_jane.py

@@ -63,14 +63,14 @@ def openrave_init(self):
     self.env.SetViewer('qtcoin')
     self.env.GetViewer().SetName('HW1 Viewer')
     self.env.Load('models/%s.env.xml' %PACKAGE_NAME)
-    cc = openravepy.RaveCreateCollisionChecker(self.env, "pqp")
-    self.env.SetCollisionChecker(cc)
-
+
     # time.sleep(3) # wait for viewer to initialize. May be helpful to uncomment
     self.robot = self.env.GetRobots()[0]
     self.manip = self.robot.GetActiveManipulator()
     self.end_effector = self.manip.GetEndEffector()
 
+    # cc = openravepy.RaveCreateCollisionChecker(self.env, 'pqp')
+    # self.env.SetCollisionChecker(cc)
 
   # problem specific initialization - load target and grasp module
   def problem_init(self):
@@ -86,7 +86,7 @@ def problem_init(self):
 
     # create a grasping module
     self.gmodel = openravepy.databases.grasping.GraspingModel(self.robot, self.target_kinbody)
-    
+
     # if you want to set options, e.g. friction
     options = openravepy.options
     options.friction = 0.1
@@ -113,7 +113,7 @@ def order_grasps_noisy(self):
     self.grasps_ordered_noisy = self.grasps_ordered.copy() #you should change the order of self.grasps_ordered_noisy
     #TODO set the score with your evaluation function (over random samples) and sort
     num_noisy_samples = 5
-    
+
 
     for grasp in self.grasps_ordered_noisy:
       noisy_samples = []
@@ -154,13 +154,13 @@ def eval_grasp(self, grasp):
 
         # Use SVD to compute minimum score
         k1 = 1
-        k2 = 1
+        k2 = 10
         k3 = 1
 
         U, S, V = np.linalg.svd(G)
         sigmaMin = abs(S[-1])
         sigmaMax = abs(S[0])
-        volumeG = np.linalg.det((G * np.transpose(G))) ** 0.5
+        volumeG = np.linalg.det(np.dot(G, np.transpose(G))) ** 0.5
         isotropy = abs(float(sigmaMin) / sigmaMax)
         print sigmaMin, volumeG, isotropy
         score = k1 * sigmaMin + k2 * volumeG + k3 * isotropy
@@ -247,17 +247,15 @@ def show_grasp(self, grasp, delay=1.5):
 if __name__ == '__main__':
   robo = RoboHandler()
 
-  # delay = 20
-  # for i in range(5):
-  #   #print 'Showing grasp ', i
-  #   #robo.show_grasp(robo.grasps_ordered[i], delay=delay)
-  #   print 'Showing noisy grasp ', i
-  #   robo.show_grasp(robo.grasps_ordered_noisy[i], delay=delay)
+  delay = 20
+  for i in range(5):
+    print 'Showing noisy grasp ', i
+    robo.show_grasp(robo.grasps_ordered_noisy[i], delay=delay)
 
 
-  #import IPython
-  #IPython.embed()
-  #print "Showing grasps..."
-  #for grasp in robo.grasps_ordered:
+  # import IPython
+  # IPython.embed()
+  # print "Showing grasps..."
+  # for grasp in robo.grasps_ordered:
   # robo.show_grasp(grasp)
-  #time.sleep(10000) #to keep the openrave window open
+  # time.sleep(10000) #to keep the openrave window open