From 40aa24ea5c47e495fe7d06ef9680d3358673860f Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Thu, 10 Dec 2020 20:09:07 +0900 Subject: [PATCH 1/2] [rtm-ros-robot-interface.l] add start/stop-auto-balancer-odom-safe --- .../euslisp/rtm-ros-robot-interface.l | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l index da2cf0d79..756ab61bd 100644 --- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l +++ b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l @@ -40,6 +40,8 @@ (ros::subscribe (format nil "/~A_contact_states" x) hrpsys_ros_bridge::ContactStatesStamped #'send self :rtmros-contact-states-callback (read-from-string (format nil ":~A-contact-states" x)) :groupname groupname)) '(ref act)) + (ros::subscribe "/odom" nav_msgs::Odometry + #'send self :rtmros-abc-odom-callback :groupname groupname) )) (:rtmros-motor-states-callback (msg) @@ -77,6 +79,14 @@ (ref-act msg) (let ((ss (send-all (send msg :states) :state :state))) (send self :set-robot-state1 ref-act ss))) + (:rtmros-abc-odom-callback + (msg) + (let ((parsed + (list + (cons :stamp (send msg :header :stamp)) + (cons :pose (ros::tf-pose->coords (send msg :pose :pose))) + (cons :twist (ros::tf-twist->coords (send msg :twist :twist)))))) + (send self :set-robot-state1 :abc-odom parsed))) (:tmp-force-moment-vector-for-limb (f/m fsensor-name &optional (topic-name-prefix nil)) ;; topic-name-prefix is "off" or "reference" (let ((key-name (if topic-name-prefix @@ -2001,6 +2011,22 @@ (dolist (limb ic-limbs) (send self :wait-impedance-controller-transition limb)) ) + ;; Start AutoBalancer while keeping continuity of odometry + ;; Foot-mid-coords have to be horizontal. + (:start-auto-balancer-odom-safe + (&rest args) + (send* self :start-auto-balancer args)) + ;; Stop AutoBalancer while keeping continuity of odometry + ;; You have to stop ImpedanceController and Stabilizer in advance + (:stop-auto-balancer-odom-safe + () + (send self :angle-vector-sequence-full + (list (map float-vector #'rad2deg (send (send (send self :robothardwareservice_getStatus2) :rs) :command))) + (list 5000) + :root-coords (list (send self :state :abc-odom :pose))) + (send self :wait-interpolation-seq) + (send self :stop-auto-balancer) + ) ) ;; ReferenceForceUpdater From 7445105140720fd940b5ede33915293b6c802c42 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Fri, 11 Dec 2020 15:00:27 +0900 Subject: [PATCH 2/2] fix --- hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l index 756ab61bd..bcdabfe2e 100644 --- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l +++ b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l @@ -2011,18 +2011,18 @@ (dolist (limb ic-limbs) (send self :wait-impedance-controller-transition limb)) ) - ;; Start AutoBalancer while keeping continuity of odometry - ;; Foot-mid-coords have to be horizontal. (:start-auto-balancer-odom-safe (&rest args) + "Start AutoBalancer while keeping continuity of odometry + Foot-mid-coords have to be horizontal." (send* self :start-auto-balancer args)) - ;; Stop AutoBalancer while keeping continuity of odometry - ;; You have to stop ImpedanceController and Stabilizer in advance (:stop-auto-balancer-odom-safe () + "Stop AutoBalancer while keeping continuity of odometry + You have to stop ImpedanceController and Stabilizer in advance" (send self :angle-vector-sequence-full (list (map float-vector #'rad2deg (send (send (send self :robothardwareservice_getStatus2) :rs) :command))) - (list 5000) + (list 1) ;; This value have to be equal to or smaller than the control period of hrpsys :root-coords (list (send self :state :abc-odom :pose))) (send self :wait-interpolation-seq) (send self :stop-auto-balancer)