Go as fast as possible!

Siddhartha Srinivasa · Siddhartha Srinivasa · commit 5313100e10ff · 2015-03-26T11:17:33.000-04:00
diff --git a/src/prpy/planning/vectorfield.py b/src/prpy/planning/vectorfield.py
@@ -78,6 +78,9 @@ def vf_geodesic():
                                             goal_pose)
             dqout, tout = prpy.util.ComputeJointVelocityFromTwist(
                                 robot, twist)
+            # Go as fast as possible
+            dqout = min(abs(robot.GetDOFVelocityLimits()/dqout))*dqout
+
             return dqout
 
         def CloseEnough():
@@ -138,6 +141,9 @@ def vf_straightline():
             twist[0:3] = direction
             dqout, tout = prpy.util.ComputeJointVelocityFromTwist(
                     robot, twist)
+
+            # Go as fast as possible
+            dqout = min(abs(robot.GetDOFVelocityLimits()/dqout))*dqout
             return dqout
 
         def TerminateMove():
