Skip to content

Commit 2cb9163

Browse files
committed
Merge pull request #80 from personalrobotics/feature/avoid_joint_limit_twist
feature added to avoid joint limit with ComputeJointVelocityFromTwist.
2 parents 8f62401 + dd17787 commit 2cb9163

File tree

1 file changed

+39
-8
lines changed

1 file changed

+39
-8
lines changed

src/prpy/util.py

Lines changed: 39 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -469,17 +469,45 @@ def get_duration(self):
469469
return self.end - self.start
470470

471471

472-
def quadraticObjective(dq, *args):
472+
def quadraticPlusJointLimitObjective(dq, J, dx, q, q_min, q_max, delta_joint_penalty=5e-1, lambda_dqdist=0.01, *args):
473+
'''
474+
Quadratic plus joint limit avoidance objective function for SciPy's optimization.
475+
@param dq joint velocity
476+
@param J Jacobian
477+
@param dx desired twist
478+
@param q current joint values
479+
@param q_min lower joint limit
480+
@param q_max upper joint limit
481+
@param delta_joint_penalty distance from limit with penality
482+
@param lamdbda_dqdist weighting for joint limit penalty
483+
'''
484+
485+
#compute quadratic distance part
486+
objective, gradient = quadraticObjective(dq, J, dx)
487+
488+
#add penalty for joint limit avoidance
489+
qdiff_lower = delta_joint_penalty - (q-q_min)
490+
qdiff_upper = delta_joint_penalty - (q_max-q)
491+
492+
dq_target = [diff_lower if diff_lower > 0. else
493+
-diff_upper if diff_upper > 0. else
494+
0. for diff_lower, diff_upper in zip(qdiff_lower, qdiff_upper)]
495+
496+
objective += lambda_dqdist * 0.5 * sum( numpy.square(dq - dq_target))
497+
gradient += lambda_dqdist * (dq-dq_target)
498+
499+
return objective, gradient
500+
501+
502+
def quadraticObjective(dq, J, dx, *args):
473503
'''
474504
Quadratic objective function for SciPy's optimization.
475505
@param dq joint velocity
476-
@param args[0]Jacobian
477-
@param args[1] desired twist
506+
@param J Jacobian
507+
@param dx desired twist
478508
@return objective the objective function
479509
@return gradient the analytical gradient of the objective
480510
'''
481-
J = args[0]
482-
dx = args[1]
483511
error = (numpy.dot(J, dq) - dx)
484512
objective = 0.5*numpy.dot(numpy.transpose(error), error)
485513
gradient = numpy.dot(numpy.transpose(J), error)
@@ -488,6 +516,7 @@ def quadraticObjective(dq, *args):
488516

489517
def ComputeJointVelocityFromTwist(robot, twist,
490518
objective=quadraticObjective,
519+
joint_limit_tolerance=3e-2,
491520
dq_init=None):
492521
'''
493522
Computes the optimal joint velocity given a twist by formulating
@@ -500,6 +529,8 @@ def ComputeJointVelocityFromTwist(robot, twist,
500529
defaults to quadraticObjective
501530
@params dq_init optional initial guess for optimal joint velocity
502531
defaults to robot.GetActiveDOFVelocities()
532+
@params joint_limit_tolerance if less then this distance to joint
533+
limit, velocity is bounded in that direction to 0
503534
@return dq_opt optimal joint velocity
504535
@return twist_opt actual achieved twist
505536
can be different from desired twist due to constraints
@@ -519,15 +550,15 @@ def ComputeJointVelocityFromTwist(robot, twist,
519550
# Check for joint limits
520551
q_curr = robot.GetActiveDOFValues()
521552
q_min, q_max = robot.GetActiveDOFLimits()
522-
dq_bounds = [(0, max) if (numpy.isclose(q_curr[i], q_min[i])) else
523-
(min, 0) if (numpy.isclose(q_curr[i], q_max[i])) else
553+
dq_bounds = [(0, max) if (numpy.isclose(q_curr[i], q_min[i], atol=joint_limit_tolerance)) else
554+
(min, 0) if (numpy.isclose(q_curr[i], q_max[i], atol=joint_limit_tolerance)) else
524555
(min, max) for i, (min, max) in enumerate(bounds)]
525556

526557
if dq_init is None:
527558
dq_init = robot.GetActiveDOFVelocities()
528559

529560
opt = scipy.optimize.fmin_l_bfgs_b(objective, dq_init, fprime=None,
530-
args=(jacobian_active, twist_active),
561+
args=(jacobian_active, twist_active, q_curr, q_min, q_max),
531562
bounds=dq_bounds, approx_grad=False)
532563

533564
dq_opt = opt[0]

0 commit comments

Comments
 (0)