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

move 360 rotatoin problem from pr2-interface.l to robot-interface.l #319

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

move 360 rotatoin problem from pr2-interface.l to robot-interface.l #319

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
54e4860
remove code for 360 rotatoin problem fro pr2-interface.l
k-okada Oct 1, 2017
7cb48da
robot-interface.l: add 360 rotatoin code for arm with continuous joints
k-okada Oct 1, 2017
de34beb
Merge branch 'master' into fix_360
k-okada Feb 11, 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
26 changes: 26 additions & 0 deletions pr2eus/robot-interface.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -370,6 +370,17 @@
(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))
"Send joint angle to robot, this method returns immediately, so use :wait-interpolation to block until the motion stops.
Expand Down Expand Up @@ -410,6 +421,21 @@
(ros::ros-error ":angle-vector tm is invalid. args: ~A" tm)
(error ":angle-vector tm is invalid. args: ~A" tm)))
)
;; when ther robot have continuous joint
;; workaround for unintentional 360 joint rotation problem [https://github.com/jsk-ros-pkg/jsk_pr2eus/commit/ed5c6dffe19f3cdf7a7f9b253c9172e120065ef8]
(when (some #'identity (mapcan #'(lambda (param)
(mapcar #'(lambda (name) (let ((j (send robot :joint name)))
(and (eq (send j :max-angle) *inf*)
(eq (send j :min-angle) *-inf*))))
(cdr (assoc :joint-names param)))) ctype))
(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)))))
;; for simulation mode
(when (send self :simulation-modep)
(if av (send self :angle-vector-simulation av tm ctype)))
Expand Down