Merge pull request #177 from personalrobotics/bugfix/ComputeJointVelo…

…cityFromTwist Fixed sign on tolerance in ComputeJointVelocityFromTwist.
personalrobotics · Sep 13, 2015 · 587c9c0 · 587c9c0
2 parents 40ac3d5 + 5f2df07
commit 587c9c0
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: