Fixed sign on tolerance in ComputeJointVelocityFromTwist.

personalrobotics · Sep 13, 2015 · 5f2df07 · 5f2df07
1 parent 40ac3d5
commit 5f2df07
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/src/prpy/util.py b/src/prpy/util.py
@@ -612,7 +612,7 @@ def ComputeJointVelocityFromTwist(robot, twist,
     q_curr = robot.GetActiveDOFValues()
     q_min, q_max = robot.GetActiveDOFLimits()
     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, 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: