Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[WIP] auto-stabilizer for jaxon weel #1122

Open
wants to merge 40 commits into
base: master
Choose a base branch
from
Open
Changes from all commits
Commits
Show all changes
40 commits
Select commit Hold shift + click to select a range
40afb9c
add landing position port
YutaKojio May 24, 2019
1a20171
publish next footstep rot
YutaKojio May 31, 2019
64f94dc
publish steppable region
YutaKojio Jun 5, 2019
7bff1a5
publish end_cog_state
YutaKojio Aug 23, 2019
aa1b144
publish is_stuck state
YutaKojio Dec 13, 2019
54dfbbe
publish estimated fxy
YutaKojio Dec 13, 2019
dfb0453
publish /use_flywheel
YutaKojio Jan 9, 2020
ca54b04
remove stabilizer service
YutaKojio Jun 2, 2020
63a8d78
fix bug; enable set st param from eus
YutaKojio Nov 11, 2020
7ad0940
[rtm-ros-robot-interface.l]fix bug in :cmd-vel-cb
Naoki-Hiraoka Dec 1, 2020
c5691b9
[rtm-ros-robot-interface.l]add comment to :go-velocity
Naoki-Hiraoka Dec 1, 2020
dbbf0c8
[rtm-ros-robot-interface.l] fix bug in :cmd-vel-mode
Naoki-Hiraoka Dec 1, 2020
624afed
Merge pull request #1 from Naoki-Hiraoka/auto-stabilizer
YutaKojio Dec 1, 2020
1cebe4e
publish current steppable region for debug
YutaKojio Dec 11, 2020
facea76
[HrpsysSeqStateROSBridge.cpp] set body to reference angle when Goal c…
Naoki-Hiraoka Dec 29, 2020
f81550b
support samplerobot
YutaKojio Jun 8, 2021
8eef1f8
Merge branch 'master' of github.com:start-jsk/rtmros_common into auto…
YutaKojio Jul 17, 2021
f889b03
add go-wheel
YutaKojio Jun 16, 2021
a6a117f
consider z in steppable region
YutaKojio Aug 18, 2021
4c60446
fix bug on vs_num size
YutaKojio Aug 18, 2021
f0b72cb
add go-pos-wheel
YutaKojio Aug 27, 2021
d7c584f
change wheel interpolation
YutaKojio Oct 2, 2021
04da17f
change go-pos-wheel
YutaKojio Oct 2, 2021
26de395
add waitFootSteps to go-wheel
Oct 27, 2021
c14f734
[hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l] fix description…
ketaro-m Oct 28, 2021
c341efc
[hrpsys_ros_bridge/euslisp/datalogger-log-parser.l] fix :update-state…
ketaro-m Oct 28, 2021
b4a8e90
Merge branch 'auto-stabilizer' of github.com:YutaKojio/rtmros_common …
YutaKojio Nov 24, 2021
360573e
change st_* to abc_* in datalogger-log-parser.l
YutaKojio Nov 24, 2021
0adae7b
Merge branch 'fix-eus-reference-force-vector' of github.com:ketaro-m/…
YutaKojio Nov 24, 2021
e23632a
[scripts/sensor_ros_bridge_connect.py] ros topic ref_**sensor after ES
ketaro-m Nov 26, 2021
b2b0c24
Merge pull request #2 from ketaro-m/rfu-es-order
YutaKojio Nov 26, 2021
84e7fe8
Merge branch 'auto-stabilizer' of github.com:YutaKojio/rtmros_common …
YutaKojio Dec 3, 2021
cc0992e
add set-foot-step-with-wheel
YutaKojio Dec 8, 2021
d055b7e
add jump-to
YutaKojio Dec 17, 2021
d760098
merge auto-stabilizer
YutaKojio Jan 19, 2022
b3105e5
Merge pull request #3 from Naoki-Hiraoka/PR-link-Rs
YutaKojio Mar 8, 2022
d2c321b
Merge branch 'auto-stabilizer-archive' of github.com:YutaKojio/rtmros…
kindsenior Apr 22, 2022
0d0269d
Merge remote-tracking branch 'hiraoka/fix-fullbody-controller' into a…
Masanori-Konishi Jun 16, 2022
b5f5a1d
[rtm-ros-robot-interface.l] add go-wheel-no-wait
Masanori-Konishi Jun 16, 2022
3420abe
Merge pull request #3 from Masanori-Konishi/auto-stabilizer-wheel-fix…
kindsenior Jul 13, 2022
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion hrpsys_ros_bridge/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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)
24 changes: 13 additions & 11 deletions hrpsys_ros_bridge/euslisp/datalogger-log-parser.l
Original file line number Diff line number Diff line change
@@ -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
105 changes: 95 additions & 10 deletions hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l
Original file line number Diff line number Diff line change
@@ -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,29 @@
"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-no-wait
(xx rv-max ra-max)
"Call goWheel without wait."
(send self :autobalancerservice_goWheel :x xx :rv_max rv-max :ra_max ra-max))
(:go-wheel
(xx rv-max ra-max)
"Call goWheel with wait."
(send self :go-wheel-no-wait xx rv-max ra-max)
(send self :wait-foot-steps))
(:raw-get-foot-step-param
()
(send (send self :autobalancerservice_getfootstepparam) :i_param))
@@ -1359,6 +1402,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 +1837,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 +1915,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 +1961,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 +2289,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)))
20 changes: 14 additions & 6 deletions hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
Original file line number Diff line number Diff line change
@@ -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" />
9 changes: 9 additions & 0 deletions hrpsys_ros_bridge/msg/CogState.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
Header header

float64 x
float64 y
float64 z
float64 vx
float64 vy
float64 vz
int8 l_r
9 changes: 9 additions & 0 deletions hrpsys_ros_bridge/msg/LandingPosition.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
Header header

float64 x
float64 y
float64 z
float64 nx
float64 ny
float64 nz
int8 l_r
5 changes: 5 additions & 0 deletions hrpsys_ros_bridge/msg/SteppableRegion.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
Header header
geometry_msgs/PolygonStamped[] polygons
uint32[] labels
float32[] likelihood
uint32 l_r
9 changes: 4 additions & 5 deletions hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py
Original file line number Diff line number Diff line change
@@ -28,12 +28,12 @@ def connecSensorRosBridgePort(url, rh, bridge, vs, rmfo, sh, es, rfu, subscripti
print program_name, "connect ", sen.name, rmfo.port("off_" + sen.name).get_port_profile().name, bridge.port("off_" + sen.name).get_port_profile().name
connectPorts(rmfo.port("off_" + sen.name), bridge.port("off_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy) # for abs forces
if sen.type == 'Force' and bridge.port("ref_" + sen.name): # for reference forces
if rfu != None and rfu.port("ref_"+sen.name+"Out"):
print program_name, "connect ", sen.name, rfu.port("ref_"+sen.name+"Out").get_port_profile().name, bridge.port("ref_" + sen.name).get_port_profile().name
connectPorts(rfu.port("ref_"+sen.name+"Out"), bridge.port("ref_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy)
elif es != None and es.port(sen.name+"Out"):
if es != None and es.port(sen.name+"Out"):
print program_name, "connect ", sen.name, es.port(sen.name+"Out").get_port_profile().name, bridge.port("ref_" + sen.name).get_port_profile().name
connectPorts(es.port(sen.name+"Out"), bridge.port("ref_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy)
elif rfu != None and rfu.port("ref_"+sen.name+"Out"):
print program_name, "connect ", sen.name, rfu.port("ref_"+sen.name+"Out").get_port_profile().name, bridge.port("ref_" + sen.name).get_port_profile().name
connectPorts(rfu.port("ref_"+sen.name+"Out"), bridge.port("ref_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy)
elif sh.port(sen.name+"Out"):
print program_name, "connect ", sen.name, sh.port(sen.name+"Out").get_port_profile().name, bridge.port("ref_" + sen.name).get_port_profile().name
connectPorts(sh.port(sen.name+"Out"), bridge.port("ref_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy)
@@ -78,4 +78,3 @@ def initSensorRosBridgeConnection(url, simulator_name, rosbridge_name, managerho
initSensorRosBridgeConnection(sys.argv[1], sys.argv[2], sys.argv[3], sys.argv[4], sys.argv[5], sys.argv[6], sys.argv[7])
else :
print program_name, " requires url, simulator_name, rosbridge_name"

164 changes: 164 additions & 0 deletions hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
Original file line number Diff line number Diff line change
@@ -62,6 +62,8 @@ HrpsysSeqStateROSBridge::HrpsysSeqStateROSBridge(RTC::Manager* manager) :
joint_controller_state_pub = nh.advertise<control_msgs::JointTrajectoryControllerState>("/fullbody_controller/state", 1);
#endif
trajectory_command_sub = nh.subscribe("/fullbody_controller/command", 1, &HrpsysSeqStateROSBridge::onTrajectoryCommandCB, this);
landing_height_sub = nh.subscribe("/landing_height", 1, &HrpsysSeqStateROSBridge::onLandingHeightCB, this);
steppable_region_sub = nh.subscribe("/steppable_region", 1, &HrpsysSeqStateROSBridge::onSteppableRegionCB, this);
mot_states_pub = nh.advertise<hrpsys_ros_bridge::MotorStates>("/motor_states", 1);
odom_pub = nh.advertise<nav_msgs::Odometry>("/odom", 1);
imu_pub = nh.advertise<sensor_msgs::Imu>("/imu", 1);
@@ -158,12 +160,18 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onInitialize() {
ref_contact_states_pub = nh.advertise<hrpsys_ros_bridge::ContactStatesStamped>("/ref_contact_states", 10);
act_contact_states_pub = nh.advertise<hrpsys_ros_bridge::ContactStatesStamped>("/act_contact_states", 10);
cop_pub.resize(m_mcforceName.size());
landing_target_pub = nh.advertise<hrpsys_ros_bridge::LandingPosition>("/landing_target", 10);
end_cog_state_pub = nh.advertise<hrpsys_ros_bridge::CogState>("/end_cog_state", 10);
for (unsigned int i=0; i<m_mcforceName.size(); i++){
std::string tmpname(m_mcforceName[i]); // "ref_xx"
tmpname.erase(0,4); // Remove "ref_"
cop_pub[i] = nh.advertise<geometry_msgs::PointStamped>(tmpname+"_cop", 10);
}
em_mode_pub = nh.advertise<std_msgs::Int32>("emergency_mode", 10);
is_stuck_pub = nh.advertise<std_msgs::Int32>("is_stuck", 10);
use_flywheel_pub = nh.advertise<std_msgs::Int32>("use_flywheel", 10);
estimated_fxy_pub = nh.advertise<geometry_msgs::PointStamped>("estimated_fxy", 10);
current_steppable_region_pub = nh.advertise<hrpsys_ros_bridge::SteppableRegion>("current_steppable_region", 10);

return RTC::RTC_OK;
}
@@ -183,6 +191,13 @@ void HrpsysSeqStateROSBridge::onJointTrajectory(trajectory_msgs::JointTrajectory

std::vector<std::string> joint_names = trajectory.joint_names;

// set body to current reference angle
if ( m_mcangle.data.length() == body->joints().size() ) {
for (unsigned int i = 0; i < body->joints().size(); i++){
body->joint(i)->q = m_mcangle.data[i];
}
}

for (unsigned int i=0; i < trajectory.points.size(); i++) {
angles[i].length(body->joints().size());

@@ -275,6 +290,33 @@ void HrpsysSeqStateROSBridge::onTrajectoryCommandCB(const trajectory_msgs::Joint
onJointTrajectory(*msg);
}

void HrpsysSeqStateROSBridge::onLandingHeightCB(const hrpsys_ros_bridge::LandingPosition::ConstPtr& msg) {
m_rslandingHeight.data.x = msg->x;
m_rslandingHeight.data.y = msg->y;
m_rslandingHeight.data.z = msg->z;
m_rslandingHeight.data.nx = msg->nx;
m_rslandingHeight.data.ny = msg->ny;
m_rslandingHeight.data.nz = msg->nz;
m_rslandingHeight.data.l_r = msg->l_r;
m_rslandingHeightOut.write();
}

void HrpsysSeqStateROSBridge::onSteppableRegionCB(const hrpsys_ros_bridge::SteppableRegion::ConstPtr& msg) {
size_t convex_num(msg->polygons.size());
m_rssteppableRegion.data.region.length(convex_num);
for (size_t i = 0; i < convex_num; i++) {
size_t vs_num(msg->polygons[i].polygon.points.size());
m_rssteppableRegion.data.region[i].length(3 * vs_num); // x,y,z components
for (size_t j = 0; j < vs_num; j++) {
m_rssteppableRegion.data.region[i][3*j] = msg->polygons[i].polygon.points[j].x;
m_rssteppableRegion.data.region[i][3*j+1] = msg->polygons[i].polygon.points[j].y;
m_rssteppableRegion.data.region[i][3*j+2] = msg->polygons[i].polygon.points[j].z;
}
}
m_rssteppableRegion.data.l_r = msg->l_r;
m_rssteppableRegionOut.write();
}

bool HrpsysSeqStateROSBridge::setSensorTransformation(hrpsys_ros_bridge::SetSensorTransformation::Request& req,
hrpsys_ros_bridge::SetSensorTransformation::Response& res)
{
@@ -797,6 +839,53 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
}
}

if ( m_rslandingTargetIn.isNew() ) {
try {
m_rslandingTargetIn.read();
hrpsys_ros_bridge::LandingPosition landingTargetv;
if ( use_hrpsys_time ) {
landingTargetv.header.stamp = ros::Time(m_rslandingTarget.tm.sec, m_rslandingTarget.tm.nsec);
}else{
landingTargetv.header.stamp = tm_on_execute;
}
landingTargetv.header.frame_id = rootlink_name;
landingTargetv.x = m_rslandingTarget.data.x;
landingTargetv.y = m_rslandingTarget.data.y;
landingTargetv.z = m_rslandingTarget.data.z;
landingTargetv.l_r = m_rslandingTarget.data.l_r;
landing_target_pub.publish(landingTargetv);
}
catch(const std::runtime_error &e)
{
ROS_ERROR_STREAM("[" << getInstanceName() << "] " << e.what());
}
}

if ( m_rsendCogStateIn.isNew() ) {
try {
m_rsendCogStateIn.read();
hrpsys_ros_bridge::CogState endCogStatev;
if ( use_hrpsys_time ) {
endCogStatev.header.stamp = ros::Time(m_rsendCogState.tm.sec, m_rsendCogState.tm.nsec);
}else{
endCogStatev.header.stamp = tm_on_execute;
}
endCogStatev.header.frame_id = rootlink_name;
endCogStatev.x = m_rsendCogState.data.x;
endCogStatev.y = m_rsendCogState.data.y;
endCogStatev.z = m_rsendCogState.data.z;
endCogStatev.vx = m_rsendCogState.data.vx;
endCogStatev.vy = m_rsendCogState.data.vy;
endCogStatev.vz = m_rsendCogState.data.vz;
endCogStatev.l_r = m_rsendCogState.data.l_r;
end_cog_state_pub.publish(endCogStatev);
}
catch(const std::runtime_error &e)
{
ROS_ERROR_STREAM("[" << getInstanceName() << "] " << e.what());
}
}

if ( m_refContactStatesIn.isNew() && m_controlSwingSupportTimeIn.isNew() ) {
try {
m_refContactStatesIn.read();
@@ -902,6 +991,81 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
}
}

if ( m_isStuckIn.isNew() ) {
try {
m_isStuckIn.read();
std_msgs::Int32 is_stuck;
is_stuck.data = m_isStuck.data;
is_stuck_pub.publish(is_stuck);
}
catch(const std::runtime_error &e)
{
ROS_ERROR_STREAM("[" << getInstanceName() << "] " << e.what());
}
}

if ( m_useFlywheelIn.isNew() ) {
try {
m_useFlywheelIn.read();
std_msgs::Int32 use_flywheel;
use_flywheel.data = m_useFlywheel.data;
use_flywheel_pub.publish(use_flywheel);
}
catch(const std::runtime_error &e)
{
ROS_ERROR_STREAM("[" << getInstanceName() << "] " << e.what());
}
}

if ( m_estimatedFxyIn.isNew() ) {
try {
m_estimatedFxyIn.read();
geometry_msgs::PointStamped fxyv;
if ( use_hrpsys_time ) {
fxyv.header.stamp = ros::Time(m_estimatedFxy.tm.sec, m_estimatedFxy.tm.nsec);
}else{
fxyv.header.stamp = tm_on_execute;
}
fxyv.header.frame_id = rootlink_name;
fxyv.point.x = m_estimatedFxy.data.x;
fxyv.point.y = m_estimatedFxy.data.y;
fxyv.point.z = m_estimatedFxy.data.z;
estimated_fxy_pub.publish(fxyv);
}
catch(const std::runtime_error &e)
{
ROS_ERROR_STREAM("[" << getInstanceName() << "] " << e.what());
}
}

if ( m_currentSteppableRegionIn.isNew() ) {
try {
m_currentSteppableRegionIn.read();
hrpsys_ros_bridge::SteppableRegion region;
bool is_valid = false;
size_t convex_num(m_currentSteppableRegion.data.region.length());
region.polygons.resize(convex_num);
for (size_t i = 0; i < convex_num; i++) {
size_t vs_num(m_currentSteppableRegion.data.region[i].length()/3);
if (vs_num >= 3) is_valid = true;
region.polygons[i].polygon.points.resize(vs_num);
for (size_t j = 0; j < vs_num; j++) {
region.polygons[i].polygon.points[j].x = m_currentSteppableRegion.data.region[i][3*j];
region.polygons[i].polygon.points[j].y = m_currentSteppableRegion.data.region[i][3*j+1];
region.polygons[i].polygon.points[j].z = m_currentSteppableRegion.data.region[i][3*j+2];
}
}
region.l_r = m_currentSteppableRegion.data.l_r;
if (is_valid) current_steppable_region_pub.publish(region);
}
catch(const std::runtime_error &e)
{
ROS_ERROR_STREAM("[" << getInstanceName() << "] " << e.what());
}
}



//
return RTC::RTC_OK;
}
9 changes: 7 additions & 2 deletions hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
Original file line number Diff line number Diff line change
@@ -30,6 +30,9 @@
#include "dynamic_reconfigure/Reconfigure.h"
#include "hrpsys_ros_bridge/MotorStates.h"
#include "hrpsys_ros_bridge/ContactStatesStamped.h"
#include "hrpsys_ros_bridge/LandingPosition.h"
#include "hrpsys_ros_bridge/CogState.h"
#include "hrpsys_ros_bridge/SteppableRegion.h"
#include "diagnostic_msgs/DiagnosticArray.h"
#include "sensor_msgs/Imu.h"
#include "hrpsys_ros_bridge/SetSensorTransformation.h"
@@ -54,14 +57,16 @@ class HrpsysSeqStateROSBridge : public HrpsysSeqStateROSBridgeImpl
void onFollowJointTrajectoryActionGoal();
void onFollowJointTrajectoryActionPreempt();
void onTrajectoryCommandCB(const trajectory_msgs::JointTrajectoryConstPtr& msg);
void onLandingHeightCB(const hrpsys_ros_bridge::LandingPosition::ConstPtr& msg);
void onSteppableRegionCB(const hrpsys_ros_bridge::SteppableRegion::ConstPtr& msg);
bool sendMsg (dynamic_reconfigure::Reconfigure::Request &req,
dynamic_reconfigure::Reconfigure::Response &res);
bool setSensorTransformation(hrpsys_ros_bridge::SetSensorTransformation::Request& req,
hrpsys_ros_bridge::SetSensorTransformation::Response& res);
private:
ros::NodeHandle nh;
ros::Publisher joint_state_pub, joint_controller_state_pub, mot_states_pub, diagnostics_pub, clock_pub, zmp_pub, ref_cp_pub, act_cp_pub, odom_pub, imu_pub, em_mode_pub, ref_contact_states_pub, act_contact_states_pub;
ros::Subscriber trajectory_command_sub;
ros::Publisher joint_state_pub, joint_controller_state_pub, mot_states_pub, diagnostics_pub, clock_pub, zmp_pub, ref_cp_pub, act_cp_pub, odom_pub, imu_pub, em_mode_pub, ref_contact_states_pub, act_contact_states_pub, landing_target_pub, end_cog_state_pub, is_stuck_pub, use_flywheel_pub, estimated_fxy_pub, current_steppable_region_pub;
ros::Subscriber trajectory_command_sub, landing_height_sub, steppable_region_sub;
std::vector<ros::Publisher> fsensor_pub, cop_pub;
#ifdef USE_PR2_CONTROLLERS_MSGS
actionlib::SimpleActionServer<pr2_controllers_msgs::JointTrajectoryAction> joint_trajectory_server;
16 changes: 16 additions & 0 deletions hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp
Original file line number Diff line number Diff line change
@@ -41,10 +41,18 @@ HrpsysSeqStateROSBridgeImpl::HrpsysSeqStateROSBridgeImpl(RTC::Manager* manager)
m_rsactCPIn("rsactCapturePoint", m_rsactCP),
m_rsCOPInfoIn("rsCOPInfo", m_rsCOPInfo),
m_emergencyModeIn("emergencyMode", m_emergencyMode),
m_isStuckIn("isStuck", m_isStuck),
m_useFlywheelIn("useFlywheel", m_useFlywheel),
m_estimatedFxyIn("estimatedFxy", m_estimatedFxy),
m_refContactStatesIn("refContactStates", m_refContactStates),
m_actContactStatesIn("actContactStates", m_actContactStates),
m_controlSwingSupportTimeIn("controlSwingSupportTime", m_controlSwingSupportTime),
m_mctorqueOut("mctorque", m_mctorque),
m_rslandingTargetIn("rslandingTarget", m_rslandingTarget),
m_rsendCogStateIn("rsendCogState", m_rsendCogState),
m_rslandingHeightOut("rslandingHeight", m_rslandingHeight),
m_rssteppableRegionOut("rssteppableRegion", m_rssteppableRegion),
m_currentSteppableRegionIn("currentSteppableRegion", m_currentSteppableRegion),
m_SequencePlayerServicePort("SequencePlayerService")

// </rtc-template>
@@ -73,12 +81,20 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridgeImpl::onInitialize()
addInPort("servoState", m_servoStateIn);
addInPort("rsCOPInfo", m_rsCOPInfoIn);
addInPort("emergencyMode", m_emergencyModeIn);
addInPort("isStuck", m_isStuckIn);
addInPort("useFlywheel", m_useFlywheelIn);
addInPort("estimatedFxy", m_estimatedFxyIn);
addInPort("refContactStates", m_refContactStatesIn);
addInPort("actContactStates", m_actContactStatesIn);
addInPort("controlSwingSupportTime", m_controlSwingSupportTimeIn);
addInPort("rslandingTarget", m_rslandingTargetIn);
addInPort("rsendCogState", m_rsendCogStateIn);
addInPort("currentSteppableRegion", m_currentSteppableRegionIn);

// Set OutPort buffer
addOutPort("mctorque", m_mctorqueOut);
addOutPort("rslandingHeight", m_rslandingHeightOut);
addOutPort("rssteppableRegion", m_rssteppableRegionOut);

// Set service provider to Ports

16 changes: 16 additions & 0 deletions hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h
Original file line number Diff line number Diff line change
@@ -153,17 +153,33 @@ class HrpsysSeqStateROSBridgeImpl : public RTC::DataFlowComponentBase
InPort<TimedDoubleSeq> m_rsCOPInfoIn;
TimedLong m_emergencyMode;
InPort<TimedLong> m_emergencyModeIn;
TimedBoolean m_isStuck;
InPort<TimedBoolean> m_isStuckIn;
TimedBoolean m_useFlywheel;
InPort<TimedBoolean> m_useFlywheelIn;
TimedPoint3D m_estimatedFxy;
InPort<TimedPoint3D> m_estimatedFxyIn;
TimedBooleanSeq m_refContactStates, m_actContactStates;
InPort<TimedBooleanSeq> m_refContactStatesIn, m_actContactStatesIn;
TimedDoubleSeq m_controlSwingSupportTime;
InPort<TimedDoubleSeq> m_controlSwingSupportTimeIn;
OpenHRP::TimedLandingPosition m_rslandingTarget;
InPort<OpenHRP::TimedLandingPosition> m_rslandingTargetIn;
OpenHRP::TimedCogState m_rsendCogState;
InPort<OpenHRP::TimedCogState> m_rsendCogStateIn;
OpenHRP::TimedSteppableRegion m_currentSteppableRegion;
InPort<OpenHRP::TimedSteppableRegion> m_currentSteppableRegionIn;

// </rtc-template>

// DataOutPort declaration
// <rtc-template block="outport_declare">
TimedDoubleSeq m_mctorque;
OutPort<TimedDoubleSeq> m_mctorqueOut;
OpenHRP::TimedLandingPosition m_rslandingHeight;
OutPort<OpenHRP::TimedLandingPosition> m_rslandingHeightOut;
OpenHRP::TimedSteppableRegion m_rssteppableRegion;
OutPort<OpenHRP::TimedSteppableRegion> m_rssteppableRegionOut;

// </rtc-template>

49 changes: 37 additions & 12 deletions hrpsys_ros_bridge/test/samplerobot_hrpsys_config.py
Original file line number Diff line number Diff line change
@@ -30,25 +30,27 @@ def setStAbcParameters (self):
tlp.debug_print_freq = int(10 / 0.002)
tlp.alarmRatio = 1.0
self.tl_svc.setParameter(tlp)
abcp=self.abc_svc.getAutoBalancerParam()[1]
abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY
# ST parameters
stp=self.st_svc.getParameter()
stp.st_algorithm=OpenHRP.StabilizerService.EEFMQP
stp=self.abc_svc.getStabilizerParam()
stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP
# eefm st params
tmp_leg_inside_margin=71.12*1e-3
tmp_leg_outside_margin=71.12*1e-3
tmp_leg_front_margin=182.0*1e-3
tmp_leg_rear_margin=72.0*1e-3
rleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_inside_margin]),
OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_outside_margin]),
OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_outside_margin]),
OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_inside_margin])]
lleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_outside_margin]),
OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]),
OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]),
OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])]
rleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_inside_margin]),
OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_outside_margin]),
OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_outside_margin]),
OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_inside_margin])]
lleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_outside_margin]),
OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]),
OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]),
OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])]
rarm_vertices = rleg_vertices
larm_vertices = lleg_vertices
stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices])
stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.AutoBalancerService.SupportPolygonVertices(vertices=x), [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices])
stp.eefm_leg_inside_margin=tmp_leg_inside_margin
stp.eefm_leg_outside_margin=tmp_leg_outside_margin
stp.eefm_leg_front_margin=tmp_leg_front_margin
@@ -63,7 +65,30 @@ def setStAbcParameters (self):
stp.k_tpcc_x=[4.0, 4.0]
stp.k_brot_p=[0.0, 0.0]
stp.k_brot_tc=[0.1, 0.1]
self.st_svc.setParameter(stp)
self.abc_svc.setStabilizerParam(stp)
gg=self.abc_svc.getGaitGeneratorParam()[1]
gg.default_step_time=1.0
gg.default_step_height=0.05
gg.default_double_support_ratio=0.15
gg.swing_trajectory_delay_time_offset=0.15
gg.stair_trajectory_way_point_offset=[0.03, 0.0, 0.0]
gg.swing_trajectory_final_distance_weight=3.0
gg.leg_margin=[0.18, 0.07, 0.07, 0.07]
gg.safe_leg_margin=[0.15, 0.05, 0.05, 0.05]
gg.stride_limitation_for_circle_type=[0.15, 0.3, 15, 0.1, 0.13]
gg.overwritable_stride_limitation=[0.35, 0.4, 0, 0.35, 0.12]
gg.margin_time_ratio=0.3
gg.use_act_states=False
gg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE
gg.default_orbit_type = OpenHRP.AutoBalancerService.RECTANGLE
gg.toe_pos_offset_x = 1e-3*117.338;
gg.heel_pos_offset_x = 1e-3*-116.342;
gg.toe_zmp_offset_x = 1e-3*117.338;
gg.heel_zmp_offset_x = 1e-3*-116.342;
gg.optional_go_pos_finalize_footstep_num=1
gg.overwritable_footstep_index_offset=1
self.abc_svc.setGaitGeneratorParam(gg)


def __init__(self, robotname=""):
HrpsysConfigurator.__init__(self)