@@ -469,17 +469,45 @@ def get_duration(self):
469
469
return self .end - self .start
470
470
471
471
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 ):
473
503
'''
474
504
Quadratic objective function for SciPy's optimization.
475
505
@param dq joint velocity
476
- @param args[0] Jacobian
477
- @param args[1] desired twist
506
+ @param J Jacobian
507
+ @param dx desired twist
478
508
@return objective the objective function
479
509
@return gradient the analytical gradient of the objective
480
510
'''
481
- J = args [0 ]
482
- dx = args [1 ]
483
511
error = (numpy .dot (J , dq ) - dx )
484
512
objective = 0.5 * numpy .dot (numpy .transpose (error ), error )
485
513
gradient = numpy .dot (numpy .transpose (J ), error )
@@ -488,6 +516,7 @@ def quadraticObjective(dq, *args):
488
516
489
517
def ComputeJointVelocityFromTwist (robot , twist ,
490
518
objective = quadraticObjective ,
519
+ joint_limit_tolerance = 3e-2 ,
491
520
dq_init = None ):
492
521
'''
493
522
Computes the optimal joint velocity given a twist by formulating
@@ -500,6 +529,8 @@ def ComputeJointVelocityFromTwist(robot, twist,
500
529
defaults to quadraticObjective
501
530
@params dq_init optional initial guess for optimal joint velocity
502
531
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
503
534
@return dq_opt optimal joint velocity
504
535
@return twist_opt actual achieved twist
505
536
can be different from desired twist due to constraints
@@ -519,15 +550,15 @@ def ComputeJointVelocityFromTwist(robot, twist,
519
550
# Check for joint limits
520
551
q_curr = robot .GetActiveDOFValues ()
521
552
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
524
555
(min , max ) for i , (min , max ) in enumerate (bounds )]
525
556
526
557
if dq_init is None :
527
558
dq_init = robot .GetActiveDOFVelocities ()
528
559
529
560
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 ),
531
562
bounds = dq_bounds , approx_grad = False )
532
563
533
564
dq_opt = opt [0 ]
0 commit comments