Merge pull request #789 from knorth55/support-tm-fast

[baxtereus] support tm :fast in moveit angle-vector methods

jsk_baxter_robot/baxtereus/baxter-interface.l

@@ -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))

jsk_baxter_robot/baxtereus/test/test-baxter-moveit.l

@@ -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)))
@@ -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)