diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index ab6b83f10..d2d975c1b 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -347,7 +347,7 @@ (:angle-vector-duration (start end &optional (scale 1) (min-time 1.0) (ctype controller-type)) - (let* ((unordered-joint-names + (let* ((unordered-joint-names (flatten (mapcar #'(lambda (joint-param) (cdr (assoc :joint-names joint-param))) (send self ctype)))) @@ -395,6 +395,7 @@ - min-time : minimum time for time to goal - end-coords-interpolation : set t if you want to move robot in cartesian space interpolation - end-coords-interpolation-steps : number of divisions when interpolating end-coords +- limit-rotation : set t if you want to restrict upper/lower limit angle of unlimited rotational joint " (if end-coords-interpolation (return-from :angle-vector (send self :angle-vector-sequence (list av) (list tm) ctype start-time :scale scale :min-time min-time :end-coords-interpolation t :end-coords-interpolation-steps end-coords-interpolation-steps))) @@ -407,7 +408,7 @@ (warn ";; controller-type: ~A not found" ctype) (return-from :angle-vector)) ;;Check and decide tm - (let ((fastest-tm (* 1000 (send self :angle-vector-duration + (let ((fastest-tm (* 1000 (send self :angle-vector-duration (send self :state :potentio-vector) av scale min-time ctype)))) (cond ;;Fastest time Mode