Skip to content

Commit

Permalink
Merge pull request #789 from knorth55/support-tm-fast
Browse files Browse the repository at this point in the history 
[baxtereus] support tm :fast in moveit angle-vector methods
  • Loading branch information
@k-okada
k-okada authored Jun 28, 2017
2 parents 905f4c9 + 1668ad6 commit ec22898
Show file tree
Hide file tree
Showing 2 changed files with 140 additions and 50 deletions.
48 changes: 22 additions & 26 deletions jsk_baxter_robot/baxtereus/baxter-interface.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -243,54 +243,50 @@
&key (move-arm :arms) (start-offset-time 0.01) (clear-velocities t) &allow-other-keys)
"Send joind angle to robot with self-collision motion planning, this method returns immediately, so use :wait-interpolation to block until the motion stops.
- av : joint angle vector [rad]
- tm : (time to goal in [msec]) ;; currently this value is ignored
- tm : time to goal in [msec]
"
(setq ctype (or ctype controller-type)) ;; use default if ctype is nil
;; for simulation mode
(when (send self :simulation-modep)
(return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args)))
(if (and (get self :moveit-environment)
(send (get self :moveit-environment) :robot))
(if (or (numberp tm) (null tm))
(progn
(unless tm (setq tm 3000))
(send-super* :angle-vector-motion-plan av :ctype ctype :move-arm move-arm :total-time tm
:start-offset-time start-offset-time :clear-velocities clear-velocities
:use-torso nil args))
(progn
(warning-message 3 ":angle-vector tm is not a number, execute :angle-vector-raw instead~%")
(unless tm (setq tm :fast))
(return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args))))
(progn
(unless tm (setq tm 3000))
(send-super* :angle-vector-motion-plan av :ctype ctype :move-arm move-arm :total-time tm
:start-offset-time start-offset-time :clear-velocities clear-velocities
:use-torso nil args))
(progn
(warning-message 3 "moveit environment is not correctly set, execute :angle-vector-raw instead~%")
(unless tm (setq tm :fast))
(return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args)))))
(:angle-vector-sequence
(avs &optional tm (ctype controller-type) (start-time 0) &rest args
(avs &optional tms (ctype controller-type) (start-time 0) &rest args
&key (move-arm :arms) (start-offset-time 0.01) (clear-velocities t) &allow-other-keys)
"Send joind angle sequence to robot with self-collision motion planning, this method returns immediately, so use :wait-interpolation to block until the motion stops.
- avs : sequence of joint angle vector [rad]
- tms : list of time to goal from previous angle-vector point in [msec]
"
(setq ctype (or ctype controller-type)) ;; use default if ctype is nil
;; for simulation mode
(when (send self :simulation-modep)
(return-from :angle-vector-sequence
(send* self :angle-vector-sequence-raw avs tm ctype start-time args)))
(send* self :angle-vector-sequence-raw avs tms ctype start-time args)))
(if (and (get self :moveit-environment)
(send (get self :moveit-environment) :robot))
(if (or (numberp tm) (null tm))
(progn
(unless tm (setq tm 3000))
(send-super* :angle-vector-motion-plan avs :ctype ctype :move-arm move-arm :total-time tm
:start-offset-time start-offset-time :clear-velocities clear-velocities
:use-torso nil args))
(progn
(warning-message 3 ":angle-vector-sequence tm is not a number, execute :angle-vector-sequence-raw instead~%")
(unless tm (setq tm :fast))
(return-from :angle-vector-sequence
(send* self :angle-vector-sequence-raw avs tm ctype start-time args))))
(progn
(setq tms
(if tms
(if (and (listp tms) (every #'numberp tms)) (apply #'+ tms) tms)
3000))
(send-super* :angle-vector-motion-plan avs :ctype ctype :move-arm move-arm :total-time tms
:start-offset-time start-offset-time :clear-velocities clear-velocities
:use-torso nil args))
(progn
(warning-message 3 "moveit environment is not correctly set, execute :angle-vector-sequence-raw instead~%")
(unless tm (setq tm :fast))
(unless tms (setq tms :fast))
(return-from :angle-vector-sequence
(send* self :angle-vector-sequence-raw avs tm ctype start-time args)))))
(send* self :angle-vector-sequence-raw avs tms ctype start-time args)))))
(:ros-state-callback
(msg)
(let ((robot-msg-names (send msg :name)) (torso-index))
Expand Down
142 changes: 118 additions & 24 deletions jsk_baxter_robot/baxtereus/test/test-baxter-moveit.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,8 +8,8 @@
(ros::load-ros-manifest "baxtereus")
(ros::roseus "baxter-moveit")

(deftest test-baxter-moveit ()
(let ((waiting-count 0) ri robot av)
(deftest test-baxter-moveit-init ()
(let ((waiting-count 0))
(while
(null (and (one-shot-subscribe "/clock" rosgraph_msgs::Clock :timeout 3000)
(one-shot-subscribe "/robot/state" baxter_core_msgs::AssemblyState :timeout 3000)))
Expand All @@ -28,34 +28,128 @@
(unix::sleep 3))
(ros::ros-info "moveit ready")

(setq ri (instance baxter-interface :init))
(setq robot (instance baxter-robot :init))
(send ri :angle-vector-raw (send robot :reset-pose))
(send ri :wait-interpolation)
(setq *ri* (instance baxter-interface :init))
(setq *baxter* (instance baxter-robot :init))))

(setq av (send ri :angle-vector (send robot :init-pose) 3000))
(send ri :wait-interpolation)
(assert av "failed: (send ri :angle-vector (send robot :init-pose) 3000)")
(deftest test-baxter-moveit-angle-vector()
(let (av)
(send *ri* :angle-vector-raw (send *baxter* :reset-pose))
(send *ri* :wait-interpolation)

(setq av (send ri :angle-vector (send robot :untuck-pose) 3000 :rarm-controller 0))
(send ri :wait-interpolation)
(assert av "failed: (send ri :angle-vector (send robot :untuck-pose) 3000 :rarm-controller 0)")
(setq av (send *ri* :angle-vector (send *baxter* :init-pose) 3000))
(send *ri* :wait-interpolation)
(assert av "failed: (send *ri* :angle-vector (send *baxter* :init-pose) 3000)")

(setq av (send ri :angle-vector (send robot :untuck-pose) 3000 :larm-controller 0))
(send ri :wait-interpolation)
(assert av "failed: (send ri :angle-vector (send robot :untuck-pose) 3000 :larm-controller 0)")
(setq av (send *ri* :angle-vector (send *baxter* :untuck-pose) 3000 :rarm-controller 0))
(send *ri* :wait-interpolation)
(assert av "failed: (send *ri* :angle-vector (send *baxter* :untuck-pose) 3000 :rarm-controller 0)")

(setq av (send ri :angle-vector (send robot :init-pose) 3000 :rarm-controller 0 :move-arm :rarm))
(send ri :wait-interpolation)
(assert av "failed: (send ri :angle-vector (send robot :init-pose) 3000 :rarm-controller 0 :move-arm :rarm)")
(setq av (send *ri* :angle-vector (send *baxter* :untuck-pose) 3000 :larm-controller 0))
(send *ri* :wait-interpolation)
(assert av "failed: (send *ri* :angle-vector (send *baxter* :untuck-pose) 3000 :larm-controller 0)")

(setq av (send ri :angle-vector (send robot :init-pose) 3000 :larm-controller 0 :move-arm :larm))
(send ri :wait-interpolation)
(assert av "failed: (send ri :angle-vector (send robot :init-pose) 3000 :larm-controller 0 :move-arm :larm)")
(setq av (send *ri* :angle-vector (send *baxter* :init-pose) 3000 :rarm-controller 0 :move-arm :rarm))
(send *ri* :wait-interpolation)
(assert av "failed: (send *ri* :angle-vector (send *baxter* :init-pose) 3000 :rarm-controller 0 :move-arm :rarm)")

(setq av (send ri :angle-vector (send robot :untuck-pose) 3000 nil 0 :move-arm :arms))
(send ri :wait-interpolation)
(assert av "failed: (send ri :angle-vector (send robot :untuck-pose) 3000 nil 0 :move-arm :arms)")
(setq av (send *ri* :angle-vector (send *baxter* :init-pose) 3000 :larm-controller 0 :move-arm :larm))
(send *ri* :wait-interpolation)
(assert av "failed: (send *ri* :angle-vector (send *baxter* :init-pose) 3000 :larm-controller 0 :move-arm :larm)")

(setq av (send *ri* :angle-vector (send *baxter* :untuck-pose) 3000 nil 0 :move-arm :arms))
(send *ri* :wait-interpolation)
(assert av "failed: (send *ri* :angle-vector (send *baxter* :untuck-pose) 3000 nil 0 :move-arm :arms)")
))

(deftest test-baxter-moveit-angle-vector-fast()
(let (av)
(send *ri* :angle-vector-raw (send *baxter* :reset-pose))
(send *ri* :wait-interpolation)

(setq av (send *ri* :angle-vector (send *baxter* :init-pose) :fast))
(send *ri* :wait-interpolation)
(assert av "failed: (send *ri* :angle-vector (send *baxter* :init-pose) :fast)")

(setq av (send *ri* :angle-vector (send *baxter* :untuck-pose) :fast :rarm-controller 0))
(send *ri* :wait-interpolation)
(assert av "failed: (send *ri* :angle-vector (send *baxter* :untuck-pose) :fast :rarm-controller 0)")

(setq av (send *ri* :angle-vector (send *baxter* :untuck-pose) :fast :larm-controller 0))
(send *ri* :wait-interpolation)
(assert av "failed: (send *ri* :angle-vector (send *baxter* :untuck-pose) :fast :larm-controller 0)")

(setq av (send *ri* :angle-vector (send *baxter* :init-pose) :fast :rarm-controller 0 :move-arm :rarm))
(send *ri* :wait-interpolation)
(assert av "failed: (send *ri* :angle-vector (send *baxter* :init-pose) :fast :rarm-controller 0 :move-arm :rarm)")

(setq av (send *ri* :angle-vector (send *baxter* :init-pose) :fast :larm-controller 0 :move-arm :larm))
(send *ri* :wait-interpolation)
(assert av "failed: (send *ri* :angle-vector (send *baxter* :init-pose) :fast :larm-controller 0 :move-arm :larm)")

(setq av (send *ri* :angle-vector (send *baxter* :untuck-pose) :fast nil 0 :move-arm :arms))
(send *ri* :wait-interpolation)
(assert av "failed: (send *ri* :angle-vector (send *baxter* :untuck-pose) :fast nil 0 :move-arm :arms)")
))


(deftest test-baxter-moveit-angle-vector-sequence ()
(let (avs)
(send *ri* :angle-vector-raw (send *baxter* :reset-pose))
(send *ri* :wait-interpolation)

(setq avs (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) 3000))
(send *ri* :wait-interpolation)
(assert avs "failed: (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) 3000)")

(setq avs (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) 3000 :rarm-controller 0))
(send *ri* :wait-interpolation)
(assert avs "failed: (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) 3000 :rarm-controller 0)")

(setq avs (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) 3000 :larm-controller 0))
(send *ri* :wait-interpolation)
(assert avs "failed: (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) 3000 :larm-controller 0)")

(setq avs (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) 3000 :rarm-controller 0 :move-arm :rarm))
(send *ri* :wait-interpolation)
(assert avs "failed: (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) 3000 :rarm-controller 0)")

(setq avs (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) 3000 :larm-controller 0 :move-arm :larm))
(send *ri* :wait-interpolation)
(assert avs "failed: (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) 3000 :larm-controller 0)")

(setq avs (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) 3000 nil 0 :move-arm :arms))
(send *ri* :wait-interpolation)
(assert avs "failed: (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) 3000 nil 0 :move-arm :arms)")
))

(deftest test-baxter-moveit-angle-vector-sequence-fast ()
(let (avs)
(send *ri* :angle-vector-raw (send *baxter* :reset-pose))
(send *ri* :wait-interpolation)

(setq avs (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) :fast))
(send *ri* :wait-interpolation)
(assert avs "failed: (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) :fast)")

(setq avs (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) :fast :rarm-controller 0))
(send *ri* :wait-interpolation)
(assert avs "failed: (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) :fast :rarm-controller 0)")

(setq avs (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) :fast :larm-controller 0))
(send *ri* :wait-interpolation)
(assert avs "failed: (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) :fast :larm-controller 0)")

(setq avs (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) :fast :rarm-controller 0 :move-arm :rarm))
(send *ri* :wait-interpolation)
(assert avs "failed: (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) :fast :rarm-controller 0)")

(setq avs (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) :fast :larm-controller 0 :move-arm :larm))
(send *ri* :wait-interpolation)
(assert avs "failed: (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) :fast :larm-controller 0)")

(setq avs (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) :fast nil 0 :move-arm :arms))
(send *ri* :wait-interpolation)
(assert avs "failed: (send *ri* :angle-vector-sequence (list (send *baxter* :init-pose) (send *baxter* :untuck-pose)) :fast nil 0 :move-arm :arms)")
))

(run-all-tests)
Expand Down

0 comments on commit ec22898

Please sign in to comment.