Merge pull request #2 from knorth55/fetch-angle-vector-sequence

pass start-time to start-offset-time and support :start-time key
jsk-ros-pkg · Jun 26, 2017 · 8d25c00 · 8d25c00
2 parents 283aa9f + 60e31e9
commit 8d25c00
Showing 1 changed file with 10 additions and 12 deletions.
diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l
@@ -27,24 +27,23 @@
   (:angle-vector-sequence-raw (&rest args) (send-super* :angle-vector-sequence args))
   (:angle-vector
    (av &optional (tm 3000) &rest args) ;; (ctype controller-type) (start-time 0) &rest args
-                                       ;;  &key (use-torso t) (start-offset-time 0.01) (clear-velocities t) &allow-other-keys)
+                                       ;;  &key (use-torso t) (clear-velocities t) &allow-other-keys)
    "Send joind angle to robot with self-collision motion planning, this method retuns immediately, so use :wait-interpolation to block until the motion stops.
 - av : joint angle vector [rad]
 - tm : time to goal in [msec]
 - use-torso : set t to use torso
 "
-   (let ((ctype controller-type) (start-time 0)
-         (use-torso t) (start-offset-time 0.01) (clear-velocities t))
+   (let ((ctype controller-type) (start-time 0) (use-torso t) (clear-velocities t))
    ;; as of 0.3.x, :angle-vector (robot-interface) :acceps tm ctype start-time as optional arguments, but in here we prefer old API
    (if (= (length args) 1) ;; args must be ctype
        (setq ctype (car args)
              args (cdr args)))
-   (if (and (>= (length args) 2) (null (member (car args) '(:use-torso :start-offset-time :clear-velocities))));; args must be ctype start-time
+   (if (and (>= (length args) 2) (null (member (car args) '(:use-torso :start-time :clear-velocities))));; args must be ctype start-time
        (setq ctype (car args)
              start-time (cadr args)
              args (cddr args)))
    (if (member :use-torso args) (setq use-torso (cadr (member :use-torso args))))
-   (if (member :start-offset-time args) (setq use-torso (cadr (member :start-offset-time args))))
+   (if (member :start-time args) (setq use-torso (cadr (member :start-time args))))
    (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args))))
    ;; for simulation mode
    (when (send self :simulation-modep)
@@ -53,28 +52,27 @@
    (when (not (numberp tm))
      (ros::warn ":angle-vector tm is not a number, use :angle-vector av tm args"))
    (send* self :angle-vector-motion-plan av :ctype ctype :move-arm :rarm :total-time tm
-               :start-offset-time start-offset-time :clear-velocities clear-velocities
+               :start-offset-time start-time :clear-velocities clear-velocities
                :use-torso use-torso args)))
   (:angle-vector-sequence
    (avs &optional tms &rest args) ;; (ctype controller-type) (start-time 0) &rest args
-                                  ;; &key (use-torso t) (start-offset-time 0.01) (clear-velocities t) &allow-other-keys)
+                                  ;; &key (use-torso t) (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.
 - avs : sequence of joint angle vector [rad]
 - tms : list of time to goal from previous angle-vector point in [msec]
 - use-torso : set t to use torso
 "
-   (let ((ctype controller-type) (start-time 0)
-         (use-torso t) (start-offset-time 0.01) (clear-velocities t))
+   (let ((ctype controller-type) (start-time 0) (use-torso t) (clear-velocities t))
    ;; as of 0.3.x, :angle-vector (robot-interface) :acceps tm ctype start-time as optional arguments, but in here we prefer old API
    (if (= (length args) 1) ;; args must be ctype
        (setq ctype (car args)
              args (cdr args)))
-   (if (and (>= (length args) 2) (null (member (car args) '(:use-torso :start-offset-time :clear-velocities))));; args must be ctype start-time
+   (if (and (>= (length args) 2) (null (member (car args) '(:use-torso :start-time :clear-velocities))));; args must be ctype start-time
        (setq ctype (car args)
              start-time (cadr args)
              args (cddr args)))
    (if (member :use-torso args) (setq use-torso (cadr (member :use-torso args))))
-   (if (member :start-offset-time args) (setq use-torso (cadr (member :start-offset-time args))))
+   (if (member :start-time args) (setq use-torso (cadr (member :start-time args))))
    (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args))))
    ;; for simulation mode
    (when (send self :simulation-modep)
@@ -86,7 +84,7 @@
      (setq tms (apply #'+ tms))
      (setq tms 3000))
    (send* self :angle-vector-motion-plan avs :ctype ctype :move-arm :rarm :total-time tms
-               :start-offset-time start-offset-time :clear-velocities clear-velocities
+               :start-offset-time start-time :clear-velocities clear-velocities
                :use-torso use-torso args)))
   (:default-controller ()
    (append