From b53840d0eaf2241996815c8ccc045f22e5ba9814 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 19 Jun 2017 16:52:06 +0900 Subject: [PATCH 1/6] update :angle-vector in fetch-interface --- jsk_fetch_robot/fetcheus/fetch-interface.l | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index cb288a68e7..25a9c9bbbc 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -25,24 +25,24 @@ )) (:angle-vector-raw (&rest args) (send-super* :angle-vector args)) (:angle-vector-sequence-raw (&rest args) (send-super* :angle-vector-sequence args)) - (:angle-vector ;; this verison uses :angle-vector-sequence for sendding trajectory, this enable us to use :wait-interpolation method so we choose this for now - (av &optional (tm 3000) &rest args) + (:angle-vector + (av &optional (tm 3000) (ctype controller-type) (start-time 0) &rest args + &key (use-torso t) (start-offset-time 0.01) (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]) ;; currently this value is ignored +- tm : time to goal in [msec] - use-torso : set t to use torso " + (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 args))) ;; (when (not (numberp tm)) (ros::warn ":angle-vector tm is not a number, use :angle-vector av tm args")) - (let ((use-torso t)) - (if (and (member :use-torso args) (null (cadr (member :use-torso args)))) (setq use-torso nil)) - (send self :angle-vector-motion-plan av :move-arm :rarm :total-time tm :start-offset-time 0.01 :use-torso use-torso :clear-velocities t) - )) - ;; + (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 + :use-torso use-torso args)) (:default-controller () (append (send self :arm-controller) From 3a2abfacde09cb061cb5df509c3bbc9557fea07d Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 19 Jun 2017 16:52:42 +0900 Subject: [PATCH 2/6] add :angle-vector-sequence with moveit in fetcheus --- jsk_fetch_robot/fetcheus/fetch-interface.l | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index 25a9c9bbbc..51161d1bb5 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -43,6 +43,24 @@ (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 :use-torso use-torso args)) + (:angle-vector-sequence + (avs &optional (tm 3000) (ctype controller-type) (start-time 0) &rest args + &key (use-torso t) (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. +- avs : sequence of joint angle vector [rad] +- tm : time to goal in [msec] +- use-torso : set t to use torso +" + (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))) + (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 avs :ctype ctype :move-arm :rarm :total-time tm + :start-offset-time start-offset-time :clear-velocities clear-velocities + :use-torso use-torso args)) (:default-controller () (append (send self :arm-controller) From a7ba266e9473969ca4fba2f7152869d4cecaf891 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 21 Jun 2017 14:02:58 +0900 Subject: [PATCH 3/6] :angle-vector-sequence accept tms in list of time --- jsk_fetch_robot/fetcheus/fetch-interface.l | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index 51161d1bb5..a38ee010a0 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -44,21 +44,24 @@ :start-offset-time start-offset-time :clear-velocities clear-velocities :use-torso use-torso args)) (:angle-vector-sequence - (avs &optional (tm 3000) (ctype controller-type) (start-time 0) &rest args + (avs &optional tms (ctype controller-type) (start-time 0) &rest args &key (use-torso t) (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. - avs : sequence of joint angle vector [rad] -- tm : time to goal in [msec] +- tms : list of time to goal from previous angle-vector point in [msec] - use-torso : set t to use torso " (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))) - (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 avs :ctype ctype :move-arm :rarm :total-time tm + (send* self :angle-vector-sequence-raw avs tms ctype start-time args))) + (unless (and (listp tms) (every #'numberp tms)) + (ros::warn ":angle-vector-sequence tms is not a list of number, use :angle-vector-sequence av tms args")) + (if tms + (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 :use-torso use-torso args)) (:default-controller () From 283aa9f76c4aef683078d30dccc3dea4cf1811f7 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 22 Jun 2017 17:54:31 +0900 Subject: [PATCH 4/6] support old API --- jsk_fetch_robot/fetcheus/fetch-interface.l | 42 +++++++++++++++++----- 1 file changed, 33 insertions(+), 9 deletions(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index a38ee010a0..d95e798c23 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -26,32 +26,56 @@ (:angle-vector-raw (&rest args) (send-super* :angle-vector args)) (:angle-vector-sequence-raw (&rest args) (send-super* :angle-vector-sequence args)) (:angle-vector - (av &optional (tm 3000) (ctype controller-type) (start-time 0) &rest args - &key (use-torso t) (start-offset-time 0.01) (clear-velocities t) &allow-other-keys) + (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) "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 " - (setq ctype (or ctype controller-type)) ;; use default if ctype is nil + (let ((ctype controller-type) (start-time 0) + (use-torso t) (start-offset-time 0.01) (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 + (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 :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args)))) ;; for simulation mode (when (send self :simulation-modep) - (return-from :angle-vector (send* self :angle-vector-raw av tm args))) + (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args))) ;; (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 - :use-torso use-torso args)) + :use-torso use-torso args))) (:angle-vector-sequence - (avs &optional tms (ctype controller-type) (start-time 0) &rest args - &key (use-torso t) (start-offset-time 0.01) (clear-velocities t) &allow-other-keys) + (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) "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 " - (setq ctype (or ctype controller-type)) ;; use default if ctype is nil + (let ((ctype controller-type) (start-time 0) + (use-torso t) (start-offset-time 0.01) (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 + (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 :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args)))) ;; for simulation mode (when (send self :simulation-modep) (return-from :angle-vector-sequence @@ -63,7 +87,7 @@ (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 - :use-torso use-torso args)) + :use-torso use-torso args))) (:default-controller () (append (send self :arm-controller) From 1e59a666debae55198b0fa4dd998aaa193e8ad42 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 26 Jun 2017 16:13:04 +0900 Subject: [PATCH 5/6] pass start-time in start-offset-time --- jsk_fetch_robot/fetcheus/fetch-interface.l | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index d95e798c23..c2e4f69d8f 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -27,19 +27,18 @@ (: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 :clear-velocities))));; args must be ctype start-time (setq ctype (car args) start-time (cadr args) args (cddr args))) @@ -53,23 +52,22 @@ (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 :clear-velocities))));; args must be ctype start-time (setq ctype (car args) start-time (cadr args) args (cddr args))) @@ -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 From 60e31e917aad96c90def19b1b29099d0b49e6a2b Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 26 Jun 2017 16:13:57 +0900 Subject: [PATCH 6/6] support :start-time key in angle-vector methods --- jsk_fetch_robot/fetcheus/fetch-interface.l | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index c2e4f69d8f..6806d2ec98 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -38,12 +38,12 @@ (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 :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) @@ -67,12 +67,12 @@ (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 :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)