Merge pull request #110 from personalrobotics/bugfix/NumPyIsClose

Removed references to numpy.isclose.

Author: mkoval

src/prpy/planning/workspace.py

@@ -171,8 +171,10 @@ def PlanWorkspacePath(self, robot, traj, timelimit=5.0,
             ik_options = openravepy.IkFilterOptions.CheckEnvCollisions
 
             start_time = time.time()
+            epsilon = 1e-6
+
             try:
-                while not numpy.isclose(t, traj.GetDuration()):
+                while t < traj.GetDuration() + epsilon:
                     # Check for a timeout.
                     current_time = time.time()
                     if (timelimit is not None and

src/prpy/util.py

@@ -599,11 +599,12 @@ def ComputeJointVelocityFromTwist(robot, twist,
     jacobian_active = jacobian[rows, :]
 
     bounds = [(-x, x) for x in robot.GetActiveDOFMaxVel()]
+
     # Check for joint limits
     q_curr = robot.GetActiveDOFValues()
     q_min, q_max = robot.GetActiveDOFLimits()
-    dq_bounds = [(0, max) if (numpy.isclose(q_curr[i], q_min[i], atol=joint_limit_tolerance)) else
-                 (min, 0) if (numpy.isclose(q_curr[i], q_max[i], atol=joint_limit_tolerance)) else
+    dq_bounds = [(0, max) if q_curr[i] <= q_min[i] + joint_limit_tolerance else
+                 (min, 0) if q_curr[i] >= q_max[i] + joint_limit_tolerance else
                  (min, max) for i, (min, max) in enumerate(bounds)]
 
     if dq_init is None: