From e4c4ce9051dbf2c4a2888b9988646ca6a8753354 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Thu, 28 Sep 2017 18:11:37 +0900 Subject: [PATCH 1/6] add interpolation in :angle-vector-raw --- jsk_fetch_robot/fetcheus/fetch-interface.l | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index c0410e2b2e..4db8a590d3 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -26,7 +26,17 @@ (:state (&rest args) ":state calls with :wait-until-update by default, since Fetch publishes /joint_states from body and gripper at almost same frequency" (send-super* :state (if (member :wait-until-update args) args (append args (list :wait-until-update t))))) - (:angle-vector-raw (&rest args) (send-super* :angle-vector args)) + (:angle-vector-raw (av &optional (tm 3000) &key (devide 4) &rest args) + (let (avs tms (minjerk (instance minjerk-interpolator :init))) + (send minjerk :reset + :position-list (list (send self :state :potentio-vector :wait-until-update t) av) + :time-list (list tm)) + (send minjerk :start-interpolation) + (send minjerk :pass-time (/ tm devide)) + (dotimes (i devide) + (setq avs (cons (send minjerk :pass-time (/ tm devide)) avs))) + (setq avs (reverse avs)) + (send* self :angle-vector-sequence-raw avs (/ tm devide) args))) (: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 @@ -271,10 +281,10 @@ (unless (boundp '*fetch*) (fetch) (send *fetch* :reset-pose)) (unless (ros::ok) (ros::roseus "fetch_eus_interface")) (unless (boundp '*ri*) (setq *ri* (instance fetch-interface :init))) - + (ros::spin-once) (send *ri* :spin-once) - + (send *fetch* :angle-vector (send *ri* :state :potentio-vector)) (when create-viewer (objects (list *fetch*))) ) From 3f0863e18797a28b74bf263c7eebaf90cce67517 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Thu, 28 Sep 2017 20:21:31 +0900 Subject: [PATCH 2/6] change variable name --- jsk_fetch_robot/fetcheus/fetch-interface.l | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index 4db8a590d3..b75c05b0b4 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -26,17 +26,17 @@ (:state (&rest args) ":state calls with :wait-until-update by default, since Fetch publishes /joint_states from body and gripper at almost same frequency" (send-super* :state (if (member :wait-until-update args) args (append args (list :wait-until-update t))))) - (:angle-vector-raw (av &optional (tm 3000) &key (devide 4) &rest args) + (:angle-vector-raw (av &optional (tm 3000) &key (divide 4) &rest args) (let (avs tms (minjerk (instance minjerk-interpolator :init))) (send minjerk :reset :position-list (list (send self :state :potentio-vector :wait-until-update t) av) :time-list (list tm)) (send minjerk :start-interpolation) - (send minjerk :pass-time (/ tm devide)) - (dotimes (i devide) - (setq avs (cons (send minjerk :pass-time (/ tm devide)) avs))) + (send minjerk :pass-time (/ tm divide)) + (dotimes (i divide) + (setq avs (cons (send minjerk :pass-time (/ tm divide)) avs))) (setq avs (reverse avs)) - (send* self :angle-vector-sequence-raw avs (/ tm devide) args))) + (send* self :angle-vector-sequence-raw avs (/ tm divide) args))) (: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 From 31fcec1642d978722b3b34dbe7a08da1b5c32eac Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Thu, 28 Sep 2017 20:22:22 +0900 Subject: [PATCH 3/6] remove unused variable --- jsk_fetch_robot/fetcheus/fetch-interface.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index b75c05b0b4..4d8e63f1d8 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -27,7 +27,7 @@ ":state calls with :wait-until-update by default, since Fetch publishes /joint_states from body and gripper at almost same frequency" (send-super* :state (if (member :wait-until-update args) args (append args (list :wait-until-update t))))) (:angle-vector-raw (av &optional (tm 3000) &key (divide 4) &rest args) - (let (avs tms (minjerk (instance minjerk-interpolator :init))) + (let (avs (minjerk (instance minjerk-interpolator :init))) (send minjerk :reset :position-list (list (send self :state :potentio-vector :wait-until-update t) av) :time-list (list tm)) From d19fde151e88c280c26afdec9068bef3146f5fb9 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Thu, 28 Sep 2017 21:38:32 +0900 Subject: [PATCH 4/6] do not use variable 'divide' -> 10 (constant) --- jsk_fetch_robot/fetcheus/fetch-interface.l | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index 4d8e63f1d8..d804737784 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -26,17 +26,17 @@ (:state (&rest args) ":state calls with :wait-until-update by default, since Fetch publishes /joint_states from body and gripper at almost same frequency" (send-super* :state (if (member :wait-until-update args) args (append args (list :wait-until-update t))))) - (:angle-vector-raw (av &optional (tm 3000) &key (divide 4) &rest args) + (:angle-vector-raw (av &optional (tm 3000) &rest args) (let (avs (minjerk (instance minjerk-interpolator :init))) (send minjerk :reset :position-list (list (send self :state :potentio-vector :wait-until-update t) av) :time-list (list tm)) (send minjerk :start-interpolation) - (send minjerk :pass-time (/ tm divide)) - (dotimes (i divide) - (setq avs (cons (send minjerk :pass-time (/ tm divide)) avs))) + (send minjerk :pass-time (/ tm 10)) + (dotimes (i 10) + (setq avs (cons (send minjerk :pass-time (/ tm 10)) avs))) (setq avs (reverse avs)) - (send* self :angle-vector-sequence-raw avs (/ tm divide) args))) + (send* self :angle-vector-sequence-raw avs (/ tm 10) args))) (: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 From 2baf8fb3c08513d10362540c1544ca845c688692 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Thu, 28 Sep 2017 21:59:13 +0900 Subject: [PATCH 5/6] use 'append' instead of 'cons' --- jsk_fetch_robot/fetcheus/fetch-interface.l | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index d804737784..df1b92b3e0 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -34,9 +34,8 @@ (send minjerk :start-interpolation) (send minjerk :pass-time (/ tm 10)) (dotimes (i 10) - (setq avs (cons (send minjerk :pass-time (/ tm 10)) avs))) - (setq avs (reverse avs)) - (send* self :angle-vector-sequence-raw avs (/ tm 10) args))) + (setq avs (append avs (list (send minjerk :pass-time (/ tm 10)))))) + (send* self :angle-vector-sequence-raw avs (make-list 10 :initial-element (/ tm 10)) args))) (: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 From 9d7a737dcb456d4fc8d4cbb682bd2abffd33ba66 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Fri, 29 Sep 2017 15:43:11 +0900 Subject: [PATCH 6/6] update (*ri* . robot) at the end of :angle-vector-raw --- jsk_fetch_robot/fetcheus/fetch-interface.l | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index df1b92b3e0..9989ae3bb3 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -35,7 +35,8 @@ (send minjerk :pass-time (/ tm 10)) (dotimes (i 10) (setq avs (append avs (list (send minjerk :pass-time (/ tm 10)))))) - (send* self :angle-vector-sequence-raw avs (make-list 10 :initial-element (/ tm 10)) args))) + (send* self :angle-vector-sequence-raw avs (make-list 10 :initial-element (/ tm 10)) args) + (send robot :angle-vector (car (last avs))))) (: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 @@ -280,10 +281,10 @@ (unless (boundp '*fetch*) (fetch) (send *fetch* :reset-pose)) (unless (ros::ok) (ros::roseus "fetch_eus_interface")) (unless (boundp '*ri*) (setq *ri* (instance fetch-interface :init))) - + (ros::spin-once) (send *ri* :spin-once) - + (send *fetch* :angle-vector (send *ri* :state :potentio-vector)) (when create-viewer (objects (list *fetch*))) )