From a6fcf1dd9017a07ad35bcf0e1264f634d8be61ff Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Fri, 29 Oct 2021 21:50:13 +0900 Subject: [PATCH 01/13] add fetch servo-off method --- jsk_fetch_robot/fetcheus/fetch-interface.l | 52 +++++++++++++++++++++- 1 file changed, 50 insertions(+), 2 deletions(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index ff8eea3dc6..10f462c57c 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -4,10 +4,11 @@ (ros::load-ros-package "fetcheus") (ros::load-ros-package "fetch_driver_msgs") +(ros::load-ros-package "robot_controllers_msgs") (defclass fetch-interface :super robot-move-base-interface - :slots (gripper-action moveit-robot) + :slots (gripper-action moveit-robot controller-action) ) (defmethod fetch-interface @@ -32,6 +33,10 @@ (send self :delete-ground-collision-object) (send self :add-keepout-collision-object) (send self :add-ground-collision-object)) + (setq controller-action + (instance ros::simple-action-client :init + "/query_controller_states" + robot_controllers_msgs::QueryControllerStatesAction)) )) (:state (&rest args) "We do not have :wait-until-update option for :state :worldcoords. @@ -252,6 +257,7 @@ Example: (send self :gripper :position) => 0.00" (send *co* :delete-attached-object-by-id "keepout") (send *co* :delete-object-by-id "keepout")) (:add-ground-collision-object () + (let ((cube (make-cube 500 500 50)) (ground (make-cylinder 1000 50))) (send cube :translate #f(-20 0 0)) @@ -265,7 +271,49 @@ Example: (send self :gripper :position) => 0.00" :touch-links nil))) (:delete-ground-collision-object () (send *co* :delete-attached-object-by-id "ground") - (send *co* :delete-object-by-id "ground"))) + (send *co* :delete-object-by-id "ground")) + (:servo-off (&key (arm t) (gripper t) (head t)) + (let ((goal-servo-off nil) (state nil) (update-list nil) + (gravity_comp (list "arm_controller/gravity_compensation")) + (arm-controller (list "arm_controller/follow_joint_trajectory" + "arm_with_torso_controller/follow_joint_trajectory" + "torso_controller/follow_joint_trajectory")) + (head-controller (list "head_controller/follow_joint_trajectory" + "head_controller/point_head"))) + (when (or arm head) + (setq goal-servo-off + (instance robot_controllers_msgs::QueryControllerStatesGoal :init)) + (setq update-list (send goal-servo-off :updates)) + (when arm + (ros::ros-info "arm servo off") + ;; start gravity compensation + (dolist (controller gravity_comp) + (setq state (instance robot_controllers_msgs::ControllerState :init)) + (send state :name controller) + (send state :state 1) ;; running + (push state update-list)) + ;; stop arm controllers + (dolist (controller arm-controller) + (setq state (instance robot_controllers_msgs::ControllerState :init)) + (send state :name controller) + (send state :state 0) ;; stopping + (push state update-list))) + (when head + ;; stop head controllers + (ros::ros-info "head servo off") + (dolist (controller head-controller) + (setq state (instance robot_controllers_msgs::ControllerState :init)) + (send state :name controller) + (send state :state 0) ;; stopping + (push state update-list))) + (send goal-servo-off :updates update-list) + (send controller-action :send-goal goal-servo-off)) + (when gripper + ;; disable gripper torque + (let ((goal-gripper-servo-off (instance control_msgs::GripperCommandGoal :init))) + (ros::ros-info "gripper servo off") + (send (send goal-gripper-servo-off :command) :max_effort -1.0) + (send gripper-action :send-goal goal-gripper-servo-off)))))) ;; interface for simple base actions (defmethod fetch-interface From 555156118e25dc3d56283d533a31830d44e8b77d Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Sat, 30 Oct 2021 19:33:48 +0900 Subject: [PATCH 02/13] update variable define position fix unintended space --- 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 10f462c57c..a198ed71cb 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -257,7 +257,6 @@ Example: (send self :gripper :position) => 0.00" (send *co* :delete-attached-object-by-id "keepout") (send *co* :delete-object-by-id "keepout")) (:add-ground-collision-object () - (let ((cube (make-cube 500 500 50)) (ground (make-cylinder 1000 50))) (send cube :translate #f(-20 0 0)) @@ -273,21 +272,21 @@ Example: (send self :gripper :position) => 0.00" (send *co* :delete-attached-object-by-id "ground") (send *co* :delete-object-by-id "ground")) (:servo-off (&key (arm t) (gripper t) (head t)) - (let ((goal-servo-off nil) (state nil) (update-list nil) - (gravity_comp (list "arm_controller/gravity_compensation")) + (let ((goal-servo-off (instance robot_controllers_msgs::QueryControllerStatesGoal :init)) + (goal-gripper-servo-off (instance control_msgs::GripperCommandGoal :init)) + (state nil) (update-list nil) + (gravity-comp (list "arm_controller/gravity_compensation")) (arm-controller (list "arm_controller/follow_joint_trajectory" "arm_with_torso_controller/follow_joint_trajectory" "torso_controller/follow_joint_trajectory")) (head-controller (list "head_controller/follow_joint_trajectory" "head_controller/point_head"))) (when (or arm head) - (setq goal-servo-off - (instance robot_controllers_msgs::QueryControllerStatesGoal :init)) (setq update-list (send goal-servo-off :updates)) (when arm (ros::ros-info "arm servo off") ;; start gravity compensation - (dolist (controller gravity_comp) + (dolist (controller gravity-comp) (setq state (instance robot_controllers_msgs::ControllerState :init)) (send state :name controller) (send state :state 1) ;; running @@ -310,10 +309,9 @@ Example: (send self :gripper :position) => 0.00" (send controller-action :send-goal goal-servo-off)) (when gripper ;; disable gripper torque - (let ((goal-gripper-servo-off (instance control_msgs::GripperCommandGoal :init))) - (ros::ros-info "gripper servo off") - (send (send goal-gripper-servo-off :command) :max_effort -1.0) - (send gripper-action :send-goal goal-gripper-servo-off)))))) + (ros::ros-info "gripper servo off") + (send (send goal-gripper-servo-off :command) :max_effort -1.0) + (send gripper-action :send-goal goal-gripper-servo-off))))) ;; interface for simple base actions (defmethod fetch-interface From dfa982d4b5f71ac92e43ae060ad44ad929542772 Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Wed, 3 Nov 2021 10:43:27 +0900 Subject: [PATCH 03/13] controller-action --> fetch-controller-action --- jsk_fetch_robot/fetcheus/fetch-interface.l | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index a198ed71cb..7525273d52 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -8,7 +8,7 @@ (defclass fetch-interface :super robot-move-base-interface - :slots (gripper-action moveit-robot controller-action) + :slots (gripper-action moveit-robot fetch-controller-action) ) (defmethod fetch-interface @@ -33,7 +33,7 @@ (send self :delete-ground-collision-object) (send self :add-keepout-collision-object) (send self :add-ground-collision-object)) - (setq controller-action + (setq fetch-controller-action (instance ros::simple-action-client :init "/query_controller_states" robot_controllers_msgs::QueryControllerStatesAction)) @@ -306,7 +306,7 @@ Example: (send self :gripper :position) => 0.00" (send state :state 0) ;; stopping (push state update-list))) (send goal-servo-off :updates update-list) - (send controller-action :send-goal goal-servo-off)) + (send fetch-controller-action :send-goal goal-servo-off)) (when gripper ;; disable gripper torque (ros::ros-info "gripper servo off") From d32c84fbc254a1283a4f1867d18dcd7fb6bd560c Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Thu, 4 Nov 2021 14:47:32 +0900 Subject: [PATCH 04/13] add :servo-on method --- jsk_fetch_robot/fetcheus/fetch-interface.l | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index 7525273d52..372e5ccafa 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -311,7 +311,17 @@ Example: (send self :gripper :position) => 0.00" ;; disable gripper torque (ros::ros-info "gripper servo off") (send (send goal-gripper-servo-off :command) :max_effort -1.0) - (send gripper-action :send-goal goal-gripper-servo-off))))) + (send gripper-action :send-goal goal-gripper-servo-off)))) + (:servo-on (&key (arm t) (gripper t) (head t)) + (when arm + (ros::ros-info "arm servo on") + (send *ri* :angle-vector (send *ri* :state :potentio-vector) 3000 :arm-controller)) + (when head + (ros::ros-info "head servo on") + (send *ri* :angle-vector (send *ri* :state :potentio-vector) 3000 :head-controller)) + (when gripper + (ros::ros-info "gripper servo on") + (send *ri* :go-grasp :pos (send *ri* :gripper :position))))) ;; interface for simple base actions (defmethod fetch-interface From 3941ccb09c5b458c3473ea7c88a3b7985a007568 Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Thu, 4 Nov 2021 15:02:42 +0900 Subject: [PATCH 05/13] send current angle-vector to *fetch* --- jsk_fetch_robot/fetcheus/fetch-interface.l | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index 372e5ccafa..ea4d3a8ce5 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -315,10 +315,12 @@ Example: (send self :gripper :position) => 0.00" (:servo-on (&key (arm t) (gripper t) (head t)) (when arm (ros::ros-info "arm servo on") - (send *ri* :angle-vector (send *ri* :state :potentio-vector) 3000 :arm-controller)) + (send *fetch* :angle-vector (send *ri* :state :potentio-vector)) + (send *ri* :angle-vector (send *fetch* :angle-vector) 3000 :arm-controller)) (when head (ros::ros-info "head servo on") - (send *ri* :angle-vector (send *ri* :state :potentio-vector) 3000 :head-controller)) + (send *fetch* :angle-vector (send *ri* :state :potentio-vector)) + (send *ri* :angle-vector (send *fetch* :angle-vector) 3000 :head-controller)) (when gripper (ros::ros-info "gripper servo on") (send *ri* :go-grasp :pos (send *ri* :gripper :position))))) From eb6a7ee7f78163fe2aeb85b4a5fc529a8a74af82 Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Thu, 4 Nov 2021 15:16:21 +0900 Subject: [PATCH 06/13] add explanation of servo-on and servo-off method to README.md --- jsk_fetch_robot/README.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/jsk_fetch_robot/README.md b/jsk_fetch_robot/README.md index 0edec46a8d..de4cf3b4d9 100644 --- a/jsk_fetch_robot/README.md +++ b/jsk_fetch_robot/README.md @@ -182,6 +182,15 @@ roslaunch jsk_fetch_gazebo_demo demo.launch (send *fetch* :rarm :shoulder-y :joint-angle 75) ``` +- Servo on and off. Able to choose servo on and off part respectively. + +```lisp +(send *ri* :servo-off) ;; arm, gripper and head servos off +(send *ri* :servo-off :arm t :gripper nil :head nil) ;; arm servo off +(send *ri* :servo-on) ;; arm, gripper and head servos on +(send *ri* :servo-on :arm t :gripper nil :head nil) ;; arm servo on +``` + ### Gripper Control - Grasp object From d246eb41d924bb6b4e6bff65e8c1e0702b11fa28 Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Thu, 4 Nov 2021 16:48:19 +0900 Subject: [PATCH 07/13] fix *ri* --> self --- jsk_fetch_robot/fetcheus/fetch-interface.l | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index ea4d3a8ce5..6797cb9b1b 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -315,15 +315,13 @@ Example: (send self :gripper :position) => 0.00" (:servo-on (&key (arm t) (gripper t) (head t)) (when arm (ros::ros-info "arm servo on") - (send *fetch* :angle-vector (send *ri* :state :potentio-vector)) - (send *ri* :angle-vector (send *fetch* :angle-vector) 3000 :arm-controller)) + (send self :angle-vector (send self :state :potentio-vector) 3000 :arm-controller)) (when head (ros::ros-info "head servo on") - (send *fetch* :angle-vector (send *ri* :state :potentio-vector)) - (send *ri* :angle-vector (send *fetch* :angle-vector) 3000 :head-controller)) + (send self :angle-vector (send self :state :potentio-vector) 3000 :head-controller)) (when gripper (ros::ros-info "gripper servo on") - (send *ri* :go-grasp :pos (send *ri* :gripper :position))))) + (send self :go-grasp :pos (send self :gripper :position))))) ;; interface for simple base actions (defmethod fetch-interface From aa676c3b1646c6cc5bbf25a8d339cbad6c675ab1 Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Tue, 16 Nov 2021 17:32:02 +0900 Subject: [PATCH 08/13] start torso-controller when arm servo is on --- jsk_fetch_robot/fetcheus/fetch-interface.l | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index 6797cb9b1b..4eb7ffe34c 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -315,7 +315,8 @@ Example: (send self :gripper :position) => 0.00" (:servo-on (&key (arm t) (gripper t) (head t)) (when arm (ros::ros-info "arm servo on") - (send self :angle-vector (send self :state :potentio-vector) 3000 :arm-controller)) + (send self :angle-vector (send self :state :potentio-vector) 3000 :arm-controller) + (send self :angle-vector (send self :state :potentio-vector) 3000 :torso-controller)) (when head (ros::ros-info "head servo on") (send self :angle-vector (send self :state :potentio-vector) 3000 :head-controller)) From 2654b9d0fe0e5b524a0ad128e5e29c038a33b9b4 Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Tue, 16 Nov 2021 17:57:01 +0900 Subject: [PATCH 09/13] leave comment about servo-on --- jsk_fetch_robot/fetcheus/fetch-interface.l | 2 ++ 1 file changed, 2 insertions(+) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index 4eb7ffe34c..e9a31bfefb 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -313,6 +313,8 @@ Example: (send self :gripper :position) => 0.00" (send (send goal-gripper-servo-off :command) :max_effort -1.0) (send gripper-action :send-goal goal-gripper-servo-off)))) (:servo-on (&key (arm t) (gripper t) (head t)) + ;; the reason why :servo-off and :servo-on has different implementation is in the following issue + ;; https://github.com/fetchrobotics/robot_controllers/issues/72 (when arm (ros::ros-info "arm servo on") (send self :angle-vector (send self :state :potentio-vector) 3000 :arm-controller) From d61e00a1f972ec85155b84f244b1b00480db8269 Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Sat, 20 Nov 2021 13:43:12 +0900 Subject: [PATCH 10/13] set time as keyword option --- 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 e9a31bfefb..548d56e2b2 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -312,16 +312,16 @@ Example: (send self :gripper :position) => 0.00" (ros::ros-info "gripper servo off") (send (send goal-gripper-servo-off :command) :max_effort -1.0) (send gripper-action :send-goal goal-gripper-servo-off)))) - (:servo-on (&key (arm t) (gripper t) (head t)) + (:servo-on (&key (arm t) (gripper t) (head t) (time 500)) ;; the reason why :servo-off and :servo-on has different implementation is in the following issue ;; https://github.com/fetchrobotics/robot_controllers/issues/72 (when arm (ros::ros-info "arm servo on") - (send self :angle-vector (send self :state :potentio-vector) 3000 :arm-controller) - (send self :angle-vector (send self :state :potentio-vector) 3000 :torso-controller)) + (send self :angle-vector (send self :state :potentio-vector) time :arm-controller) + (send self :angle-vector (send self :state :potentio-vector) time :torso-controller)) (when head (ros::ros-info "head servo on") - (send self :angle-vector (send self :state :potentio-vector) 3000 :head-controller)) + (send self :angle-vector (send self :state :potentio-vector) time :head-controller)) (when gripper (ros::ros-info "gripper servo on") (send self :go-grasp :pos (send self :gripper :position))))) From 1e059b63aefe0feb937ac2ad4cf878eec7b93fd7 Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Mon, 22 Nov 2021 13:40:14 +0900 Subject: [PATCH 11/13] update default duration time of servo-on 200[msec] --- 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 548d56e2b2..03a5a4e934 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -312,7 +312,7 @@ Example: (send self :gripper :position) => 0.00" (ros::ros-info "gripper servo off") (send (send goal-gripper-servo-off :command) :max_effort -1.0) (send gripper-action :send-goal goal-gripper-servo-off)))) - (:servo-on (&key (arm t) (gripper t) (head t) (time 500)) + (:servo-on (&key (arm t) (gripper t) (head t) (time 200)) ;; the reason why :servo-off and :servo-on has different implementation is in the following issue ;; https://github.com/fetchrobotics/robot_controllers/issues/72 (when arm From 6bd1695b08080debd3467bd2dc019a48891a47df Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Mon, 22 Nov 2021 16:06:17 +0900 Subject: [PATCH 12/13] add docstring for servo-off and servo-on methods --- jsk_fetch_robot/fetcheus/fetch-interface.l | 24 ++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index 03a5a4e934..7fd3c4eb63 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -272,6 +272,17 @@ Example: (send self :gripper :position) => 0.00" (send *co* :delete-attached-object-by-id "ground") (send *co* :delete-object-by-id "ground")) (:servo-off (&key (arm t) (gripper t) (head t)) + " + Turn servo off for arm/gripper/head motors + Args: + - key + - arm: If you do not want to servo off the arm, set nil (default: t) + - gripper: If you do not want to servo off the grippper, set nil (default: t) + - head: If you do not want to servo off the head, set nil (default: t) + Examples: + (send self :servo-off) ;; arm, gripper and head servos off + (send self :servo-off :arm t :gripper nil :head nil) ;; arm servo off + " (let ((goal-servo-off (instance robot_controllers_msgs::QueryControllerStatesGoal :init)) (goal-gripper-servo-off (instance control_msgs::GripperCommandGoal :init)) (state nil) (update-list nil) @@ -313,6 +324,19 @@ Example: (send self :gripper :position) => 0.00" (send (send goal-gripper-servo-off :command) :max_effort -1.0) (send gripper-action :send-goal goal-gripper-servo-off)))) (:servo-on (&key (arm t) (gripper t) (head t) (time 200)) + " + Turn servo on for arm/gripper/head motors + Args: + - key: + - arm: If you do not want to servo on the arm, set nil (default: t) + - gripper: If you do not want to servo on the grippper, set nil (default: t) + - head: If you do not want to servo on the head, set nil (default: t) + - time: [msec] duration to turn servo on, + experimentally the minimal duration is about 60[msec] (default: 200) + Examples: + (send self :servo-on) ;; arm, gripper and head servos on + (send self :servo-on :arm nil :gripper nil :head t :time 80) ;; head servo on in 80[msec] + " ;; the reason why :servo-off and :servo-on has different implementation is in the following issue ;; https://github.com/fetchrobotics/robot_controllers/issues/72 (when arm From 6200292b4f59b2bd3172f1bbb7bb112f3f20ad79 Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Mon, 22 Nov 2021 16:09:47 +0900 Subject: [PATCH 13/13] update README.md to add keyword option in servo-on method --- jsk_fetch_robot/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_fetch_robot/README.md b/jsk_fetch_robot/README.md index de4cf3b4d9..8f26c5b220 100644 --- a/jsk_fetch_robot/README.md +++ b/jsk_fetch_robot/README.md @@ -188,7 +188,7 @@ roslaunch jsk_fetch_gazebo_demo demo.launch (send *ri* :servo-off) ;; arm, gripper and head servos off (send *ri* :servo-off :arm t :gripper nil :head nil) ;; arm servo off (send *ri* :servo-on) ;; arm, gripper and head servos on -(send *ri* :servo-on :arm t :gripper nil :head nil) ;; arm servo on +(send *ri* :servo-on :arm t :gripper nil :head nil :time 80) ;; arm servo on in 80[msec] ``` ### Gripper Control