Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[robot-interface] Add option to restrict rotation angle of unlimited rotational joint #388

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open

[robot-interface] Add option to restrict rotation angle of unlimited rotational joint #388

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
251f0ed
remove code for 360 rotatoin problem fro pr2-interface.l
k-okada Oct 1, 2017
af30282
robot-interface.l: add 360 rotatoin code for arm with continuous joints
k-okada Oct 1, 2017
cb5d5d4
add option to restrict rotation angle of unlimited rotational joint
708yamaguchi Feb 15, 2019
7b6009b
add doc string to :angle-vector in robot-interface
708yamaguchi Feb 16, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 1 addition & 22 deletions pr2eus/pr2-interface.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -395,31 +395,10 @@ Example: (send self :gripper :rarm :position) => 0.00"

;;
;;
;; workaround for unintentional 360 joint rotation problem [#91]
(defmethod pr2-interface
(:check-continuous-joint-move-over-180
(diff-av)
(let ((i 0) add-new-trajectory-point)
(dolist (j (send robot :joint-list))
;; for continuous rotational joint
(when (and (> (- (send j :max-angle) (send j :min-angle)) 360)
(> (abs (elt diff-av i)) 180))
(ros::ros-warn "continuous joint (~A) moves ~A degree, commanded joint differs from original trajectory to avoid unintentional 360 rotation" (send j :name) (elt diff-av i))
(setq add-new-trajectory-point t))
(incf i (send j :joint-dof)))
add-new-trajectory-point))
(:angle-vector
(av &optional (tm 3000) &rest args)
(let (diff-av)
;; use reference-vector to get last commanded joint and use :angle-vector to toruncate the joint limit to eus lisp style
(setq diff-av (v- av (send robot :angle-vector (send self :state :reference-vector))))
;; use shortest path for contiuous joint
;;
(when (send self :check-continuous-joint-move-over-180 diff-av)
(return-from :angle-vector
(send* self :angle-vector-sequence (list av) (list tm) args))) ;; when
(send-super* :angle-vector av tm args)
))
(send-super* :angle-vector av tm args))
(:angle-vector-sequence
(avs &optional (tms (list 3000)) &rest args)
(unless (or (send self :simulation-modep) (cadr (memq :end-coords-interpolation args)))
Expand Down
58 changes: 56 additions & 2 deletions pr2eus/robot-interface.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -370,8 +370,19 @@
(av tm ctype)
(let* ((prev-av (send robot :angle-vector)))
(send-all (gethash ctype controller-table) :push-angle-vector-simulation av tm prev-av)))
(:check-continuous-joint-move-over-180
(diff-av)
(let ((i 0) add-new-trajectory-point)
(dolist (j (send robot :joint-list))
;; for continuous rotational joint
(when (and (> (- (send j :max-angle) (send j :min-angle)) 360)
(> (abs (elt diff-av i)) 180))
(ros::ros-warn "continuous joint (~A) moves ~A degree, commanded joint differs from original trajectory to avoid unintentional 360 rotation" (send j :name) (elt diff-av i))
(setq add-new-trajectory-point t))
(incf i (send j :joint-dof)))
add-new-trajectory-point))
(:angle-vector
(av &optional (tm nil) (ctype controller-type) (start-time 0) &key (scale 1) (min-time 1.0) (end-coords-interpolation nil) (end-coords-interpolation-steps 10))
(av &optional (tm nil) (ctype controller-type) (start-time 0) &key (scale 1) (min-time 1.0) (end-coords-interpolation nil) (end-coords-interpolation-steps 10) (limit-rotation nil))
"Send joint angle to robot, this method returns immediately, so use :wait-interpolation to block until the motion stops.
- av : joint angle vector [deg]
- tm : (time to goal in [msec])
Expand All @@ -384,9 +395,14 @@
- 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)))
;; when ther robot have continuous joint
;; workaround for unintentional 360 joint rotation problem [https://github.com/jsk-ros-pkg/jsk_pr2eus/commit/ed5c6dffe19f3cdf7a7f9b253c9172e120065ef8]
(if limit-rotation
(return-from :angle-vector (send self :angle-vector-sequence (list av) (list tm) ctype start-time :scale scale :min-time min-time :limit-rotation t)))
(setq ctype (or ctype controller-type)) ;; use default controller-type if ctype is nil
(unless (gethash ctype controller-table)
(warn ";; controller-type: ~A not found" ctype)
Expand Down Expand Up @@ -427,7 +443,7 @@
cacts (send self ctype)))
av)
(:angle-vector-sequence
(avs &optional (tms (list 3000)) (ctype controller-type) (start-time 0.1) &key (scale 1) (min-time 0.0) (end-coords-interpolation nil) (end-coords-interpolation-steps 10))
(avs &optional (tms (list 3000)) (ctype controller-type) (start-time 0.1) &key (scale 1) (min-time 0.0) (end-coords-interpolation nil) (end-coords-interpolation-steps 10) (limit-rotation nil))
"Send sequence of joint angle to robot, this method returns immediately, so use :wait-interpolation to block until the motion stops.
- avs: sequence of joint angles(float-vector) [deg], (list av0 av1 ... avn)
- tms: sequence of duration(float) from previous angle-vector to next goal [msec], (list tm0 tm1 ... tmn)
Expand All @@ -441,6 +457,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
708yamaguchi marked this conversation as resolved.
Show resolved Hide resolved
"
(setq ctype (or ctype controller-type)) ;; use default controller-type if ctype is nil
(unless (gethash ctype controller-table)
Expand Down Expand Up @@ -525,6 +542,43 @@
(warning-message 1 ":angle-vector-sequence failed to generate end-coords-interpolation motion~%")
(return-from :angle-vector-sequence nil))
))
(when limit-rotation
(let* ((prev-av av-prev) (j 0) tm
avs-ret tms-ret)
(dolist (av avs)
(setq tm (nth j tms))
(setq j (+ j 1))
(let* ((scale-av (send self :sub-angle-vector av prev-av)) (diff-av scale-av)
(minjerk (instance minjerk-interpolator :init))
dist div
(i 0) (over-180 0))
;; if joint rotation pass through 180(-180) degree, change direction of rotation.
(dolist (joint (send robot :joint-list))
(setq over-180 0)
(when (and (= (send joint :min-angle) *-inf*) (= (send joint :max-angle) *inf*))
(setq over-180 (- (floor (/ (+ (aref prev-av i) (aref scale-av i) 180) 360.0)) (floor (/ (+ (aref prev-av i) 180) 360.0))))
(setf (aref diff-av i) (+ (* -360 over-180) (aref diff-av i))))
(setq i (+ i 1)))
;; if rotation angle exceeds 180, divide it into sequence.
(if (send self :check-continuous-joint-move-over-180 diff-av)
(progn
(setq dist (abs (geo::find-extream (coerce diff-av cons) #'abs #'>=)))
(setq div (round (/ dist 60.0)))
(send minjerk :reset
:position-list (list prev-av (v+ prev-av diff-av))
:time-list (list tm))
(send minjerk :start-interpolation)
(send minjerk :pass-time (/ tm div))
(dotimes (i div)
(setq avs-ret (append avs-ret (list (send minjerk :pass-time (/ tm div))))))
(setq tms-ret (append tms-ret (make-list div :initial-element (/ tm div)))))
(progn
(setq avs-ret (append avs-ret (list av)))
(setq tms-ret (append tms-ret (list tm))))))
(setq prev-av av))
(setq avs avs-ret)
(setq tms tms-ret)
))
(prog1 ;; angle-vector-sequence returns avs
avs
(while avs
Expand Down