From 8abc1f7201dd95b753524271e64828d05809e6c5 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 28 Feb 2023 14:29:15 +0900 Subject: [PATCH 1/8] [spoteus] add :move-arm-impedance-command --- jsk_spot_robot/spoteus/spot-interface.l | 25 +++++++++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index c384aa122e..22e3dd6ff1 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -54,12 +54,12 @@ (defclass spot-interface :super robot-move-base-interface - :slots (trajectory-cmd-action pick-object-in-image-client pick-object-in-image-feedback-msg) + :slots (trajectory-cmd-action pick-object-in-image-client pick-object-in-image-feedback-msg arm-impedance-command-action) ) (defmethod spot-interface (:init - (&rest args &key (trajectory-cmd-action-name "/spot/trajectory") &allow-other-keys) + (&rest args &key (trajectory-cmd-action-name "/spot/trajectory") (arm-impedance-cmd-action-name "/spot/arm_impedance_command") &allow-other-keys) (prog1 (send-super* :init :robot spot-robot :base-frame-id "base_link" :odom-topic "/odom_combined" :base-controller-action-name nil args) ;; check if spot_ros/driver.launch started @@ -81,6 +81,9 @@ (setq trajectory-cmd-action (instance ros::simple-action-client :init trajectory-cmd-action-name spot_msgs::TrajectoryAction :groupname groupname)) + (setq arm-impedance-command-action (instance ros::simple-action-client :init + arm-impedance-cmd-action-name spot_msgs::ArmImpedanceCommandAction + :groupname groupname)) (setq pick-object-in-image-client (instance ros::simple-action-client :init "/spot/pick_object_in_image" spot_msgs::PickObjectInImageAction)) )) @@ -114,6 +117,24 @@ :rotational_damping (v2v rotational-damping))) (setq r (ros::service-call "/spot/arm_impedance_parameters" m)) (send r :success)))) + (:move-arm-impedance-command (target-coords + &key + (linear-stiffness #f(500 500 500)) + (rotational-stiffness #f(60 60 60)) + (linear-damping #f(2.5 2.5 2.5)) + (rotational-damping #f(1.0 1.0 1.0))) + (let (goal + (v2v (lambda (v) (instance geometry_msgs::Vector3 :init :x (elt v 0) :y (elt v 1) :z (elt v 2)))) + ) + (setq goal (instance spot_msgs::ArmImpedanceCommandGoal :init + :target-pose (ros::coords->tf-pose target-coords) + :linear_stiffness (v2v linear-stiffness) + :rotational_stiffness (v2v rotational-stiffness) + :linear_damping (v2v linear-damping) + :rotational_damping (v2v rotational-damping))) + (send arm-impedance-command-action :send-goal goal) + (send arm-impedance-command-action :wait-for-result) + )) (:unstow-arm () (call-trigger-service "/spot/unstow_arm")) (:stow-arm () (call-trigger-service "/spot/stow_arm")) (:gripper-open () (call-trigger-service "/spot/gripper_open")) From 46895b10587282b7d3cd4e418f38101d5d7dd63b Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 28 Feb 2023 14:35:05 +0900 Subject: [PATCH 2/8] [spoteus] fix typo --- jsk_spot_robot/spoteus/spot-interface.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 22e3dd6ff1..a3059bd088 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -97,7 +97,7 @@ (list (list (cons :controller-action "spot/arm_controller/follow_joint_trajectory") - (cons :controller-state "spot/arm_controller/follow_joint_trajectory/state") + (cons :controller-state "spot/arm_controller/follow_joint_trajectory/status") (cons :action-type control_msgs::FollowJointTrajectoryAction) (cons :joint-names (send-all (send robot :arm :joint-list) :name)) ))) From fd02cb6f21ceabf620c712feb16b00726a9ae234 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 28 Feb 2023 14:39:45 +0900 Subject: [PATCH 3/8] Revert "[spoteus] fix typo" This reverts commit 2ef10bd3ae3fb290196657c795496cea71b20baa. --- jsk_spot_robot/spoteus/spot-interface.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index a3059bd088..22e3dd6ff1 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -97,7 +97,7 @@ (list (list (cons :controller-action "spot/arm_controller/follow_joint_trajectory") - (cons :controller-state "spot/arm_controller/follow_joint_trajectory/status") + (cons :controller-state "spot/arm_controller/follow_joint_trajectory/state") (cons :action-type control_msgs::FollowJointTrajectoryAction) (cons :joint-names (send-all (send robot :arm :joint-list) :name)) ))) From d907e026da515125d3bada150eecd90d6c692245 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 28 Feb 2023 15:08:21 +0900 Subject: [PATCH 4/8] [spoteus] fix bugS --- jsk_spot_robot/spoteus/spot-interface.l | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 22e3dd6ff1..6f1a7d6f5f 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -123,18 +123,18 @@ (rotational-stiffness #f(60 60 60)) (linear-damping #f(2.5 2.5 2.5)) (rotational-damping #f(1.0 1.0 1.0))) - (let (goal - (v2v (lambda (v) (instance geometry_msgs::Vector3 :init :x (elt v 0) :y (elt v 1) :z (elt v 2)))) - ) - (setq goal (instance spot_msgs::ArmImpedanceCommandGoal :init - :target-pose (ros::coords->tf-pose target-coords) - :linear_stiffness (v2v linear-stiffness) - :rotational_stiffness (v2v rotational-stiffness) - :linear_damping (v2v linear-damping) - :rotational_damping (v2v rotational-damping))) - (send arm-impedance-command-action :send-goal goal) - (send arm-impedance-command-action :wait-for-result) - )) + (let (goal) + (labels + ((v2v (v) (instance geometry_msgs::Vector3 :init :x (elt v 0) :y (elt v 1) :z (elt v 2)))) + (setq goal (instance spot_msgs::ArmImpedanceCommandGoal :init + :target_pose (ros::coords->tf-pose target-coords) + :linear_stiffness (v2v linear-stiffness) + :rotational_stiffness (v2v rotational-stiffness) + :linear_damping (v2v linear-damping) + :rotational_damping (v2v rotational-damping))) + (send arm-impedance-command-action :send-goal goal) + (send arm-impedance-command-action :wait-for-result) + ))) (:unstow-arm () (call-trigger-service "/spot/unstow_arm")) (:stow-arm () (call-trigger-service "/spot/stow_arm")) (:gripper-open () (call-trigger-service "/spot/gripper_open")) From 44318c120253de9dab25205f47aae6a1b62f4e4a Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 2 Mar 2023 12:41:43 +0900 Subject: [PATCH 5/8] [jsk_spot_startup][head_lead_demo] use :move-arm-impedance-command instead of :set-imperance-param --- .../apps/head_lead_demo/head-lead-teleop.l | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head-lead-teleop.l b/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head-lead-teleop.l index 8548d7f372..470ce89875 100755 --- a/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head-lead-teleop.l +++ b/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head-lead-teleop.l @@ -10,8 +10,8 @@ (spot-init) (if (eq (send *ri* :state :power-state-shore-power-state) 'on-shore-power) (send *ri* :undock)) -;; (send *ri* :set-impedance-param :linear-stiffness #f(250 250 250) :rotational-stiffness #f(30 30 30) :linear-damping #f(1.5 1.5 1.5) :rotational-damping #f(0.5 0.5 0.5)) -;; (send *ri* :set-impedance-param :linear-stiffness #f(250 25 25) :rotational-stiffness #f(30 5 5) :linear-damping #f(1.5 1.5 1.5) :rotational-damping #f(0.5 0.5 0.5)) +;; (send *ri* :move-arm-impedance-command (send (send *spot* :arm :endcoords) :copy-worldcoords) :linear-stiffness #f(250 250 250) :rotational-stiffness #f(30 30 30) :linear-damping #f(1.5 1.5 1.5) :rotational-damping #f(0.5 0.5 0.5)) +;; (send *ri* :move-arm-impedance-command (send (send *spot* :arm :endcoords) :copy-worldcoords) :linear-stiffness #f(250 25 25) :rotational-stiffness #f(30 5 5) :linear-damping #f(1.5 1.5 1.5) :rotational-damping #f(0.5 0.5 0.5)) (setq *default-av* #f(0.0 -130.0 120.0 0.0 10.0 0.0)) @@ -67,7 +67,9 @@ (send *ri* :angle-vector (send *spot* :angle-vector) 1000 :default-controller) (send *ri* :stop-grasp) (send *ri* :wait-interpolation) - (send *ri* :set-impedance-param + (send *spot* :angle-vector (send *ri* :state :potentio-vector)) + (send *ri* :move-arm-impedance-command + (send (send *spot* :arm :endcoords) :copy-worldcoords) :linear-stiffness #f(250 25 75) ;; z(up) y(side) x(front) :rotational-stiffness #f(3 30 30) :linear-damping #f(1.5 1.0 1.0) @@ -279,7 +281,7 @@ (setq *continue* nil)) (unix:signal unix::sigint 'ros::roseus-sigint-handler) -;; send start-func (set-impedance) when R Stick button is pressed +;; send start-func (move-arm-impedance-command) when R Stick button is pressed (ros::subscribe "/joy_dualsense" sensor_msgs::Joy #'(lambda (msg) (if (and (> (length (send msg :buttons)) 11) From 7d5796ec099b19a012e0e3eab3342a8c6429a3ab Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 2 Mar 2023 12:43:24 +0900 Subject: [PATCH 6/8] [spoteus] add docstring to move-arm-impedance-command --- jsk_spot_robot/spoteus/spot-interface.l | 1 + 1 file changed, 1 insertion(+) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 6f1a7d6f5f..40ac64192f 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -123,6 +123,7 @@ (rotational-stiffness #f(60 60 60)) (linear-damping #f(2.5 2.5 2.5)) (rotational-damping #f(1.0 1.0 1.0))) + "Move spot hand to target-coords with impedance control" (let (goal) (labels ((v2v (v) (instance geometry_msgs::Vector3 :init :x (elt v 0) :y (elt v 1) :z (elt v 2)))) From a417afb6ada3cd8332d462725889258dab6c6723 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 2 Mar 2023 12:55:23 +0900 Subject: [PATCH 7/8] [spoteus] use :move-arm-impedance-command in :set-impedance-param --- jsk_spot_robot/spoteus/spot-interface.l | 42 +++++++++++++++++-------- 1 file changed, 29 insertions(+), 13 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 40ac64192f..60f8c4b8cb 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -105,18 +105,29 @@ (&key (linear-stiffness #f(500 500 500)) (rotational-stiffness #f(60 60 60)) (linear-damping #f(2.5 2.5 2.5)) - (rotational-damping #f(1.0 1.0 1.0))) - "X is up, Z is forward" - (let (m r) - (labels - ((v2v (v) (instance geometry_msgs::Vector3 :init :x (elt v 0) :y (elt v 1) :z (elt v 2)))) - (setq m (instance spot_msgs::SetArmImpedanceParamsRequest :init - :linear_stiffness (v2v linear-stiffness) - :rotational_stiffness (v2v rotational-stiffness) - :linear_damping (v2v linear-damping) - :rotational_damping (v2v rotational-damping))) - (setq r (ros::service-call "/spot/arm_impedance_parameters" m)) - (send r :success)))) + (rotational-damping #f(1.0 1.0 1.0)) + (update-spot t)) + "Enable impedance control with current hand pose." + (let (m r temp-spot-model target-coords) + (if update-spot + (progn + (send *spot* :angle-vector (send self :state :potentio-vector)) + (setq target-coords (send (send *spot* :arm :end-coords) :copy-worldcoords)) + ) + (progn + (setq temp-spot-model (instance spot-robot :init)) + (send temp-spot-model :angle-vector (send self :state :potentio-vector)) + (setq target-coords (send (send temp-spot-model :arm :end-coords) :copy-worldcoords)) + ) + ) + (send self :move-arm-impedance-command + target-coods + :linear-stiffness linear-stiffness + :rotational-stiffness rotational-stiffness + :linear-damping linear-damping + :rotational-damping rotational-damping + ) + )) (:move-arm-impedance-command (target-coords &key (linear-stiffness #f(500 500 500)) @@ -124,7 +135,7 @@ (linear-damping #f(2.5 2.5 2.5)) (rotational-damping #f(1.0 1.0 1.0))) "Move spot hand to target-coords with impedance control" - (let (goal) + (let (goal result) (labels ((v2v (v) (instance geometry_msgs::Vector3 :init :x (elt v 0) :y (elt v 1) :z (elt v 2)))) (setq goal (instance spot_msgs::ArmImpedanceCommandGoal :init @@ -135,6 +146,11 @@ :rotational_damping (v2v rotational-damping))) (send arm-impedance-command-action :send-goal goal) (send arm-impedance-command-action :wait-for-result) + (setq result (send arm-impedance-command-action :get-result)) + (if (not (send result :success)) + (ros::ros-error (send result :message)) + ) + (send result :success) ))) (:unstow-arm () (call-trigger-service "/spot/unstow_arm")) (:stow-arm () (call-trigger-service "/spot/stow_arm")) From 5fdb90a6e0c4dd9ecd54a6a307f4e69261f445e9 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 2 Mar 2023 12:59:26 +0900 Subject: [PATCH 8/8] Revert "[jsk_spot_startup][head_lead_demo] use :move-arm-impedance-command instead of :set-imperance-param" This reverts commit 3c837f9e3f9638ff3f9a6769b8de82bdf3f12020. --- .../apps/head_lead_demo/head-lead-teleop.l | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head-lead-teleop.l b/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head-lead-teleop.l index 470ce89875..8548d7f372 100755 --- a/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head-lead-teleop.l +++ b/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head-lead-teleop.l @@ -10,8 +10,8 @@ (spot-init) (if (eq (send *ri* :state :power-state-shore-power-state) 'on-shore-power) (send *ri* :undock)) -;; (send *ri* :move-arm-impedance-command (send (send *spot* :arm :endcoords) :copy-worldcoords) :linear-stiffness #f(250 250 250) :rotational-stiffness #f(30 30 30) :linear-damping #f(1.5 1.5 1.5) :rotational-damping #f(0.5 0.5 0.5)) -;; (send *ri* :move-arm-impedance-command (send (send *spot* :arm :endcoords) :copy-worldcoords) :linear-stiffness #f(250 25 25) :rotational-stiffness #f(30 5 5) :linear-damping #f(1.5 1.5 1.5) :rotational-damping #f(0.5 0.5 0.5)) +;; (send *ri* :set-impedance-param :linear-stiffness #f(250 250 250) :rotational-stiffness #f(30 30 30) :linear-damping #f(1.5 1.5 1.5) :rotational-damping #f(0.5 0.5 0.5)) +;; (send *ri* :set-impedance-param :linear-stiffness #f(250 25 25) :rotational-stiffness #f(30 5 5) :linear-damping #f(1.5 1.5 1.5) :rotational-damping #f(0.5 0.5 0.5)) (setq *default-av* #f(0.0 -130.0 120.0 0.0 10.0 0.0)) @@ -67,9 +67,7 @@ (send *ri* :angle-vector (send *spot* :angle-vector) 1000 :default-controller) (send *ri* :stop-grasp) (send *ri* :wait-interpolation) - (send *spot* :angle-vector (send *ri* :state :potentio-vector)) - (send *ri* :move-arm-impedance-command - (send (send *spot* :arm :endcoords) :copy-worldcoords) + (send *ri* :set-impedance-param :linear-stiffness #f(250 25 75) ;; z(up) y(side) x(front) :rotational-stiffness #f(3 30 30) :linear-damping #f(1.5 1.0 1.0) @@ -281,7 +279,7 @@ (setq *continue* nil)) (unix:signal unix::sigint 'ros::roseus-sigint-handler) -;; send start-func (move-arm-impedance-command) when R Stick button is pressed +;; send start-func (set-impedance) when R Stick button is pressed (ros::subscribe "/joy_dualsense" sensor_msgs::Joy #'(lambda (msg) (if (and (> (length (send msg :buttons)) 11)