Merge pull request #62 from personalrobotics/Bugfix_GreedyIKPlanner

Fixed GreedyIKPlanner to use active DOFs.

Author: mkoval

src/prpy/planning/workspace.py

@@ -166,7 +166,7 @@ def PlanWorkspacePath(self, robot, traj, timelimit=5.0,
             dt = traj.GetDuration()
 
             # Smallest CSpace step at which to give up
-            min_step = min(robot.GetDOFResolutions())/100.
+            min_step = min(robot.GetActiveDOFResolutions())/100.
             ik_options = openravepy.IkFilterOptions.CheckEnvCollisions
 
             start_time = time.time()
@@ -192,7 +192,7 @@ def PlanWorkspacePath(self, robot, traj, timelimit=5.0,
                         step = abs(qnew - qcurr)
                         if (max(step) < min_step) and qtraj:
                             raise PlanningError('Not making progress.')
-                        infeasible_step = any(step > robot.GetDOFResolutions())
+                        infeasible_step = any(step > robot.GetActiveDOFResolutions())
                     if infeasible_step:
                         # Backtrack and try half the step
                         dt = dt/2.0