Merge pull request #80 from personalrobotics/feature/avoid_joint_limit_twist

feature added to avoid joint limit with ComputeJointVelocityFromTwist.

Author: mkoval

src/prpy/util.py

@@ -469,17 +469,45 @@ def get_duration(self):
         return self.end - self.start
 
 
-def quadraticObjective(dq, *args):
+def quadraticPlusJointLimitObjective(dq, J, dx, q, q_min, q_max, delta_joint_penalty=5e-1, lambda_dqdist=0.01, *args):
+    '''
+    Quadratic plus joint limit avoidance objective function for SciPy's optimization.
+    @param dq joint velocity
+    @param J Jacobian
+    @param dx desired twist
+    @param q current joint values
+    @param q_min lower joint limit
+    @param q_max upper joint limit
+    @param delta_joint_penalty distance from limit with penality
+    @param lamdbda_dqdist weighting for joint limit penalty
+    '''
+    
+    #compute quadratic distance part
+    objective, gradient = quadraticObjective(dq, J, dx)
+
+    #add penalty for joint limit avoidance
+    qdiff_lower = delta_joint_penalty - (q-q_min)
+    qdiff_upper = delta_joint_penalty - (q_max-q)
+
+    dq_target = [diff_lower if diff_lower > 0. else
+                 -diff_upper if diff_upper > 0. else
+                 0. for diff_lower, diff_upper in zip(qdiff_lower, qdiff_upper)]
+
+    objective += lambda_dqdist * 0.5 * sum( numpy.square(dq - dq_target))
+    gradient += lambda_dqdist * (dq-dq_target)
+          
+    return objective, gradient
+
+
+def quadraticObjective(dq, J, dx, *args):
     '''
     Quadratic objective function for SciPy's optimization.
     @param dq joint velocity
-    @param args[0]Jacobian
-    @param args[1] desired twist
+    @param J Jacobian
+    @param dx desired twist
     @return objective the objective function
     @return gradient the analytical gradient of the objective
     '''
-    J = args[0]
-    dx = args[1]
     error = (numpy.dot(J, dq) - dx)
     objective = 0.5*numpy.dot(numpy.transpose(error), error)
     gradient = numpy.dot(numpy.transpose(J), error)
@@ -488,6 +516,7 @@ def quadraticObjective(dq, *args):
 
 def ComputeJointVelocityFromTwist(robot, twist,
                                   objective=quadraticObjective,
+                                  joint_limit_tolerance=3e-2,
                                   dq_init=None):
     '''
     Computes the optimal joint velocity given a twist by formulating
@@ -500,6 +529,8 @@ def ComputeJointVelocityFromTwist(robot, twist,
             defaults to quadraticObjective
     @params dq_init optional initial guess for optimal joint velocity
             defaults to robot.GetActiveDOFVelocities()
+    @params joint_limit_tolerance if less then this distance to joint
+            limit, velocity is bounded in that direction to 0
     @return dq_opt optimal joint velocity
     @return twist_opt actual achieved twist
             can be different from desired twist due to constraints
@@ -519,15 +550,15 @@ def ComputeJointVelocityFromTwist(robot, twist,
     # 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])) else
-                 (min, 0) if (numpy.isclose(q_curr[i], q_max[i])) else
+    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
                  (min, max) for i, (min, max) in enumerate(bounds)]
 
     if dq_init is None:
         dq_init = robot.GetActiveDOFVelocities()
 
     opt = scipy.optimize.fmin_l_bfgs_b(objective, dq_init, fprime=None,
-                                       args=(jacobian_active, twist_active),
+                                       args=(jacobian_active, twist_active, q_curr, q_min, q_max),
                                        bounds=dq_bounds, approx_grad=False)
 
     dq_opt = opt[0]