[WIP] auto-stabilizer for jaxon wheel

hrpsys_ros_bridge/CMakeLists.txt

@@ -76,7 +76,7 @@ if(CCACHE_FOUND AND "$ENV{CI}" STREQUAL "true" )
 endif(CCACHE_FOUND AND "$ENV{CI}" STREQUAL "true" )
 
 # define add_message_files before rtmbuild_init
-add_message_files(FILES MotorStates.msg ContactState.msg ContactStateStamped.msg ContactStatesStamped.msg)
+add_message_files(FILES MotorStates.msg LandingPosition.msg SteppableRegion.msg CogState.msg ContactState.msg ContactStateStamped.msg ContactStatesStamped.msg)
 add_service_files(FILES SetSensorTransformation.srv)
 # initialize rtmbuild
 rtmbuild_init(geometry_msgs)

hrpsys_ros_bridge/euslisp/datalogger-log-parser.l

@@ -143,25 +143,25 @@
      (send self :set-robot-state1 :auto-balancer-end-coords-list
            (mapcar #'(lambda (x) (send robot x :end-coords :copy-worldcoords)) limb-list)))
    ;; Stabilizer parameter
-   (let ((rsd (send self :parser-list "st_originRefCogVel")))
+   (let ((rsd (send self :parser-list "abc_originRefCogVel")))
      (if rsd (send self :set-robot-state1 :stabilizer-reference-cogvel (scale 1e3 (send rsd :read-state)))))
-   (let ((rsd (send self :parser-list "st_originActCogVel")))
+   (let ((rsd (send self :parser-list "abc_originActCogVel")))
      (if rsd (send self :set-robot-state1 :stabilizer-cogvel (scale 1e3 (send rsd :read-state)))))
-   (let ((rsd (send self :parser-list "st_originRefZmp")))
+   (let ((rsd (send self :parser-list "abc_originRefZmp")))
      (if rsd (send self :set-robot-state1 :stabilizer-reference-zmp (scale 1e3 (send rsd :read-state)))))
-   (let ((rsd (send self :parser-list "st_originActZmp")))
+   (let ((rsd (send self :parser-list "abc_originActZmp")))
      (if rsd (send self :set-robot-state1 :stabilizer-zmp (scale 1e3 (send rsd :read-state)))))
-   (let ((rsd (send self :parser-list "st_originNewZmp")))
+   (let ((rsd (send self :parser-list "abc_originNewZmp")))
      (if rsd (send self :set-robot-state1 :stabilizer-newzmp (scale 1e3 (send rsd :read-state)))))
-   (let ((rsd (send self :parser-list "st_originRefCog")))
+   (let ((rsd (send self :parser-list "abc_originRefCog")))
      (if rsd (send self :set-robot-state1 :stabilizer-reference-cog (scale 1e3 (send rsd :read-state)))))
-   (let ((rsd (send self :parser-list "st_originActCog")))
+   (let ((rsd (send self :parser-list "abc_originActCog")))
      (if rsd (send self :set-robot-state1 :stabilizer-cog (scale 1e3 (send rsd :read-state)))))
-   (let ((rsd (send self :parser-list "st_actBaseRpy")))
+   (let ((rsd (send self :parser-list "abc_actBaseRpy")))
      (if rsd (send self :set-robot-state1 :stabilizer-act-base-rpy (map float-vector #'rad2deg (send rsd :read-state)))))
-   (let ((rsd (send self :parser-list "st_currentBaseRpy")))
+   (let ((rsd (send self :parser-list "abc_currentBaseRpy")))
      (if rsd (send self :set-robot-state1 :stabilizer-current-base-rpy (map float-vector #'rad2deg (send rsd :read-state)))))
-   (let ((rsd (send self :parser-list "st_debugData")))
+   (let ((rsd (send self :parser-list "abc_debugData")))
      (if rsd (send self :set-robot-state1 :stabilizer-debug-data (send rsd :read-state))))
    ;; Set stable rtc data
    (send self :set-robot-state1
@@ -207,7 +207,9 @@
            (send (send self :parser-list (format nil "rmfo_off_~A" (send f :name))) :read-state))
      (send self :set-robot-state1
            (read-from-string (format nil ":reference-~A" (string-downcase (send f :name))))
-           (send (send self :parser-list (format nil "sh_~AOut" (send f :name))) :read-state))
+           (if (send self :parser-list (format nil "rfu_ref_~AOut" (send f :name)))
+               (send (send self :parser-list (format nil "rfu_ref_~AOut" (send f :name))) :read-state)
+             (send (send self :parser-list (format nil "sh_~AOut" (send f :name))) :read-state)))
      )
    ;; (dolist (i (send robot :imu-sensors))
    ;;   (send self :set-robot-state1

hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l

@@ -32,6 +32,12 @@
                      #'send self :rtmros-imu-callback :groupname groupname)
      (ros::subscribe "/emergency_mode" std_msgs::Int32
                      #'send self :rtmros-emergency-mode-callback :groupname groupname)
+     (ros::subscribe "/is_stuck" std_msgs::Int32
+                     #'send self :rtmros-is-stuck-callback :groupname groupname)
+     (ros::subscribe "/use_flywheel" std_msgs::Int32
+                     #'send self :rtmros-use-flywheel-callback :groupname groupname)
+     (ros::subscribe "/estimated_fxy" geometry_msgs::PointStamped
+                     #'send self :rtmros-estimated-fxy-callback :groupname groupname)
      (mapcar #'(lambda (x)
                  (ros::subscribe (format nil "/~A_capture_point" x) geometry_msgs::PointStamped
                                  #'send self :rtmros-capture-point-callback (read-from-string (format nil ":~A-capture-point" x)) :groupname groupname))
@@ -69,6 +75,20 @@
    (let ((m (send msg :data)))
      (send self :set-robot-state1 :emergency-mode m))
    )
+  (:rtmros-is-stuck-callback
+   (msg)
+   (let ((m (send msg :data)))
+     (send self :set-robot-state1 :is-stuck m))
+   )
+  (:rtmros-use-flywheel-callback
+   (msg)
+   (let ((m (send msg :data)))
+     (send self :set-robot-state1 :use-flywheel m))
+   )
+  (:rtmros-estimated-fxy-callback
+   (msg)
+   (let ((p (send msg :point)))
+     (send self :set-robot-state1 :estimated-fxy (float-vector (send p :x) (send p :y) (send p :z)))))
   (:rtmros-capture-point-callback
    (ref-act msg)
    (let ((p (send msg :point)))
@@ -117,13 +137,13 @@
   (:reference-force-vector
    (&optional (limb))
    "Returns reference force-vector [N] list for all limbs obtained by :state.
-    This value corresponds to StateHolder and SequencePlayer RTC.
+    This value corresponds to ReferenceForceUpdater, EmergencyStopper, StateHolder and SequencePlayer RTC prioritized in this order.
     If a limb argument is specified, returns a vector for the limb."
    (send self :tmp-force-moment-vector :force limb "reference"))
   (:reference-moment-vector
    (&optional (limb))
    "Returns reference moment-vector [Nm] list for all limbs obtained by :state.
-    This value corresponds to StateHolder and SequencePlayer RTC.
+    This value corresponds to ReferenceForceUpdater, EmergencyStopper, StateHolder and SequencePlayer RTC prioritized in this order.
     If a limb argument is specified, returns a vector for the limb."
    (send self :tmp-force-moment-vector :moment limb "reference"))
   (:absolute-force-vector
@@ -1269,6 +1289,25 @@
    "Call goPos with wait."
    (send self :go-pos-no-wait xx yy th)
    (send self :wait-foot-steps))
+  (:jump-to-no-wait
+   (xx yy zz ts tf)
+   "Call jumpTo without wait."
+   (send self :autobalancerservice_jumpTo :x xx :y yy :z zz :ts ts :tf tf))
+  (:jump-to
+   (xx yy zz ts tf)
+   "Call jumpTo with wait."
+   (send self :jump-to-no-wait xx yy zz ts tf)
+   (send self :wait-foot-steps))
+  (:go-pos-wheel
+   (xx yy th w-xx rv-max ra-max &optional (tm-off 0.0))
+   "Call goPosWheel without wait."
+   (send self :autobalancerservice_goPosWheel :x xx :y yy :th th :w_x w-xx :rv_max rv-max :ra_max ra-max :tm_off tm-off)
+   (send self :wait-foot-steps))
+  (:go-wheel
+   (xx rv-max ra-max)
+   "Call goWheel without wait."
+   (send self :autobalancerservice_goWheel :x xx :rv_max rv-max :ra_max ra-max)
+   (send self :wait-foot-steps))
   (:raw-get-foot-step-param
    ()
    (send (send self :autobalancerservice_getfootstepparam) :i_param))
@@ -1359,6 +1398,48 @@
     For arguments, please see :set-foot-steps-with-param-no-wait documentation."
    (send self :set-foot-steps-with-param-no-wait foot-step-list step-height-list step-time-list toe-angle-list heel-angle-list :overwrite-footstep-index overwrite-footstep-index)
    (send self :wait-foot-steps))
+  (:set-foot-steps-with-wheel-no-wait
+    (foot-step-list step-height-list step-time-list toe-angle-list heel-angle-list goal-pos-list rv-max-list ra-max-list &key (overwrite-footstep-index 0))
+    (unless (listp (car foot-step-list))
+     (setq foot-step-list (mapcar #'(lambda (fs) (list fs)) foot-step-list)
+           step-height-list (mapcar #'(lambda (sh) (list sh)) step-height-list)
+           step-time-list (mapcar #'(lambda (st) (list st)) step-time-list)
+           toe-angle-list (mapcar #'(lambda (ta) (list ta)) toe-angle-list)
+           heel-angle-list (mapcar #'(lambda (ha) (list ha)) heel-angle-list)
+           goal-pos-list (mapcar #'(lambda (gp) (list gp)) goal-pos-list)
+           rv-max-list (mapcar #'(lambda (rv) (list rv)) rv-max-list)
+           ra-max-list (mapcar #'(lambda (ra) (list ra)) ra-max-list)))
+    (send self :autobalancerservice_setfootstepswithwheel
+          :fss
+          (mapcar #'(lambda (fs)
+                      (instance hrpsys_ros_bridge::openhrp_autobalancerservice_footsteps :init
+                                :fs
+                                (mapcar #'(lambda (f)
+                                            (send self :eus-footstep->abc-footstep f))
+                                        fs)))
+                  foot-step-list)
+          :spss
+          (mapcar #'(lambda (shs sts tas has)
+                      (instance hrpsys_ros_bridge::openhrp_autobalancerservice_stepparams :init
+                                :sps
+                                (mapcar #'(lambda (sh st ta ha)
+                                            (instance hrpsys_ros_bridge::openhrp_autobalancerservice_stepparam :init :step_height (* sh 1e-3) :step_time st :toe_angle ta :heel_angle ha))
+                                        shs sts tas has)))
+                  step-height-list step-time-list toe-angle-list heel-angle-list)
+          :wpss
+          (mapcar #'(lambda (gps rvs ras)
+                      (instance hrpsys_ros_bridge::openhrp_autobalancerservice_wheelparams :init
+                                :wps
+                                (mapcar #'(lambda (gp rv ra)
+                                            (instance hrpsys_ros_bridge::openhrp_autobalancerservice_wheelparam :init :goal_pos (* gp 1e-3) :rv_max rv :ra_max ra))
+                                        gps rvs ras)))
+                  goal-pos-list rv-max-list ra-max-list)
+          :overwrite_fs_idx overwrite-footstep-index
+          ))
+  (:set-foot-steps-with-wheel
+    (foot-step-list step-height-list step-time-list toe-angle-list heel-angle-list goal-pos-list rv-max-list ra-max-list &key (overwrite-footstep-index 0))
+    (send self :set-foot-steps-with-wheel-no-wait foot-step-list step-height-list step-time-list toe-angle-list heel-angle-list goal-pos-list rv-max-list ra-max-list :overwrite-footstep-index overwrite-footstep-index)
+    (send self :wait-foot-steps))
   (:set-foot-steps-roll-pitch
    (angle &key (axis :x))
    "Set foot steps with roll or pitch orientation.
@@ -1752,9 +1833,9 @@
 
 ;; StabilizerService
 (def-set-get-param-method
-  'hrpsys_ros_bridge::Openhrp_StabilizerService_stParam
+  'hrpsys_ros_bridge::Openhrp_AutoBalancerService_StabilizerParam
   :raw-set-st-param :get-st-param :get-st-param-arguments
-  :stabilizerservice_setparameter :stabilizerservice_getparameter)
+  :autobalancerservice_setstabilizerparam :autobalancerservice_getstabilizerparam)
 
 (defmethod rtm-ros-robot-interface
   (:set-st-param
@@ -1830,7 +1911,7 @@
    "Set Stabilzier parameter by default parameters."
    (unless (send self :get :default-st-param)
      (send self :put :default-st-param (send self :get-st-param)))
-   (send self :stabilizerservice_setparameter :i_param (send self :get :default-st-param))
+   (send self :autobalancerservice_setparameter :i_param (send self :get :default-st-param))
    )
   (:set-st-param-by-ratio
    (state-feedback-gain-ratio
@@ -1876,12 +1957,12 @@
   (:start-st
    ()
    "Start Stabilizer Mode."
-   (send self :stabilizerservice_startstabilizer)
+   (send self :autobalancerservice_startstabilizer)
    )
   (:stop-st
    ()
    "Stop Stabilizer Mode."
-   (send self :stabilizerservice_stopstabilizer)
+   (send self :autobalancerservice_stopstabilizer)
    )
   )
 
@@ -2204,9 +2285,9 @@
        (print-ros-msg (send self :get-gait-generator-param))
        (format t ";; :get-auto-balancer-param~%")
        (print-ros-msg (send self :get-auto-balancer-param)))
-     (when (find "/StabilizerServiceROSBridge" rosbridge-nodes :test #'string=)
-       (format t ";; :get-st-param~%")
-       (print-ros-msg (send self :get-st-param)))
+     ;; (when (find "/StabilizerServiceROSBridge" rosbridge-nodes :test #'string=)
+     ;;   (format t ";; :get-st-param~%")
+     ;;   (print-ros-msg (send self :get-st-param)))
      (when (find "/ObjectContactTurnaroundDetectorServiceROSBridge" rosbridge-nodes :test #'string=)
        (format t ";; :get-object-turnaround-detector-param~%")
        (print-ros-msg (send self :get-object-turnaround-detector-param)))

hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch

@@ -161,14 +161,18 @@
     <rtconnect from="abc.rtc:contactStates" to="HrpsysSeqStateROSBridge0.rtc:refContactStates" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
     <rtconnect from="abc.rtc:controlSwingSupportTime" to="HrpsysSeqStateROSBridge0.rtc:controlSwingSupportTime" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
     <rtconnect from="kf.rtc:rpy" to="HrpsysSeqStateROSBridge0.rtc:baseRpy" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
-    <rtconnect from="st.rtc:zmp" to="HrpsysSeqStateROSBridge0.rtc:rszmp" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
-    <rtconnect from="st.rtc:refCapturePoint" to="HrpsysSeqStateROSBridge0.rtc:rsrefCapturePoint" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
-    <rtconnect from="st.rtc:actCapturePoint" to="HrpsysSeqStateROSBridge0.rtc:rsactCapturePoint" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
-    <rtconnect from="st.rtc:actContactStates" to="HrpsysSeqStateROSBridge0.rtc:actContactStates" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
+    <rtconnect from="abc.rtc:zmpAct" to="HrpsysSeqStateROSBridge0.rtc:rszmp" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
+    <rtconnect from="abc.rtc:refCapturePoint" to="HrpsysSeqStateROSBridge0.rtc:rsrefCapturePoint" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
+    <rtconnect from="abc.rtc:actCapturePoint" to="HrpsysSeqStateROSBridge0.rtc:rsactCapturePoint" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
+    <rtconnect from="abc.rtc:actContactStates" to="HrpsysSeqStateROSBridge0.rtc:actContactStates" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
     <rtconnect from="AutoBalancerServiceROSBridge.rtc:AutoBalancerService" to="abc.rtc:AutoBalancerService"  subscription_type="new"/>
-    <rtconnect from="StabilizerServiceROSBridge.rtc:StabilizerService" to="st.rtc:StabilizerService"  subscription_type="new"/>
+    <!-- <rtconnect from="StabilizerServiceROSBridge.rtc:StabilizerService" to="st.rtc:StabilizerService"  subscription_type="new"/> -->
     <rtconnect from="KalmanFilterServiceROSBridge.rtc:KalmanFilterService" to="kf.rtc:KalmanFilterService"  subscription_type="new"/>
-    <rtconnect from="st.rtc:COPInfo" to="HrpsysSeqStateROSBridge0.rtc:rsCOPInfo" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
+    <rtconnect from="abc.rtc:COPInfo" to="HrpsysSeqStateROSBridge0.rtc:rsCOPInfo" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
+    <rtconnect from="abc.rtc:landingTarget" to="HrpsysSeqStateROSBridge0.rtc:rslandingTarget" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
+    <rtconnect from="abc.rtc:endCogState" to="HrpsysSeqStateROSBridge0.rtc:rsendCogState" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
+    <rtconnect from="HrpsysSeqStateROSBridge0.rtc:rslandingHeight" to="abc.rtc:landingHeight"  subscription_type="new"/>
+    <rtconnect from="HrpsysSeqStateROSBridge0.rtc:rssteppableRegion" to="abc.rtc:steppableRegion"  subscription_type="new"/>
     <rtactivate component="AutoBalancerServiceROSBridge.rtc" />
     <rtactivate component="StabilizerServiceROSBridge.rtc" />
     <rtactivate component="KalmanFilterServiceROSBridge.rtc" />
@@ -260,6 +264,10 @@
     <node pkg="hrpsys_ros_bridge" name="HardEmergencyStopperServiceROSBridge" type="EmergencyStopperServiceROSBridgeComp"
           output="screen" args ="$(arg openrtm_args)" />
     <rtconnect from="es.rtc:emergencyMode" to="HrpsysSeqStateROSBridge0.rtc:emergencyMode" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
+    <rtconnect from="abc.rtc:isStuck" to="HrpsysSeqStateROSBridge0.rtc:isStuck" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
+    <rtconnect from="abc.rtc:useFlywheel" to="HrpsysSeqStateROSBridge0.rtc:useFlywheel" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
+    <rtconnect from="abc.rtc:estimatedFxy" to="HrpsysSeqStateROSBridge0.rtc:estimatedFxy" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
+    <rtconnect from="abc.rtc:currentSteppableRegion" to="HrpsysSeqStateROSBridge0.rtc:currentSteppableRegion" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
     <rtconnect from="EmergencyStopperServiceROSBridge.rtc:EmergencyStopperService" to="es.rtc:EmergencyStopperService"  subscription_type="new"/>
     <rtconnect from="HardEmergencyStopperServiceROSBridge.rtc:EmergencyStopperService" to="hes.rtc:EmergencyStopperService"  subscription_type="new"/>
     <rtactivate component="EmergencyStopperServiceROSBridge.rtc" />

hrpsys_ros_bridge/msg/CogState.msg

@@ -0,0 +1,9 @@
+Header header
+
+float64 x
+float64 y
+float64 z
+float64 vx
+float64 vy
+float64 vz
+int8 l_r

hrpsys_ros_bridge/msg/LandingPosition.msg

@@ -0,0 +1,9 @@
+Header header
+
+float64 x
+float64 y
+float64 z
+float64 nx
+float64 ny
+float64 nz
+int8 l_r

hrpsys_ros_bridge/msg/SteppableRegion.msg

@@ -0,0 +1,5 @@
+Header header
+geometry_msgs/PolygonStamped[] polygons
+uint32[] labels
+float32[] likelihood
+uint32 l_r