Fix load_link_stats function

crigroup · Jan 2, 2018 · 41fb62f · 41fb62f
1 parent 80e9961
commit 41fb62f
Showing 1 changed file with 6 additions and 3 deletions.
diff --git a/src/raveutils/kinematics.py b/src/raveutils/kinematics.py
@@ -232,15 +232,14 @@ def load_link_stats(robot, xyzdelta=0.01, autogenerate=True):
   success: bool
     `True` if succeeded, `False` otherwise
   """
-  success = False
+  success = True
   statsmodel = orpy.databases.linkstatistics.LinkStatisticsModel(robot)
   if not statsmodel.load() and autogenerate:
     print 'Generating LinkStatistics database. Will take ~1 minute...'
     statsmodel.autogenerate()
   if statsmodel.load():
     statsmodel.setRobotWeights()
     statsmodel.setRobotResolutions(xyzdelta=xyzdelta)
-    success = True
   else:
     manip = robot.GetActiveManipulator()
     indices = manip.GetArmIndices()
@@ -254,7 +253,11 @@ def load_link_stats(robot, xyzdelta=0.01, autogenerate=True):
       jweights[i] = np.sum(jweights[i:])
     robot_weights = np.ones(robot.GetDOF())
     robot_weights[indices] = np.array(jweights) / np.max(jweights)
-    robot.SetDOFWeights(robot_weights)
+    if np.all(robot_weights >= br._EPS):
+      # All the weights have to be greater than 0
+      robot.SetDOFWeights(robot_weights)
+    else:
+      success = False
   return success
 
 def random_joint_values(robot):