samplerobotは歩くがhrp2がgazebo上で歩かない

[otsubo]

roslaunch hrpsys_gazebo_tutorials gazebo_hrp2jsknt_no_controllers.launch
rtmlaunch hrpsys_gazebo_tutorials hrp2jsknt_bring_up.launch

を上げたて,eusで

roseus hrp2jsknt-interface.l
(hrp2jsknt-init)
(setq *robot* *hrp2jsknt*)
(send *ri* :start-auto-balancer)
(send *ri* :angle-vector (send *robot* :reset-pose) 4000)
(send *ri* :wait-interpolation)
(send *ri* :set-st-param :st-algorithm :tpcc)
(send *ri* :set-kalman-filter-param :acc-offset #f(0 0 0) :sensorrpy-offset #f(0 0 0)))
(send *ri* :start-st)

を実行するとこけてしまいます.
samplerobotで同じように実行したところ,歩けました.
チェックポイント等はありますでしょうか.
よろしくお願いいたします.

[snozawa]

rosversion hrpsys
rosversion hrpsys_ros_bridge
rosversion hrpsys_ros_bridge_tutorials
rosversion hrpsys_gazebo_general
rosversion hrpsys_gazebo_tutorials

を教えてください。

[snozawa]

また、eusで軌道してコケた後で

(pprint (send (send *ri* :get-st-param) :slots))

を教えてください。

[otsubo]

leus@leus-brix-gpu:~$ rosversion hrpsys
315.7.0
leus@leus-brix-gpu:~$ rosversion hrpsys_ros_bridge
1.2.14
leus@leus-brix-gpu:~$ rosversion hrpsys_ros_bridge_tutorials
0.1.6
leus@leus-brix-gpu:~$ rosversion hrpsys_gazebo_general
0.1.9
leus@leus-brix-gpu:~$ rosversion hrpsys_gazebo_tutorials
0.1.6

です．

[snozawa]

stのパラメータの他、

(pprint (send (send *ri* :get-st-param) :slots))
(pprint (send (send *ri* :get-auto-balancer-param) :slots))
(pprint (send (send *ri* :get-gait-generator-param) :slots))
(pprint (send (send *ri* :get-kalman-fileter-param) :slots))

も教えてください。

[otsubo]

26.E4-irteusgl$ (pprint (send (send *ri* :get-st-param) :slots))
((plist)
 (ros::_k_tpcc_p . #f(2.0 2.0))
 (ros::_k_tpcc_x . #f(5.0 5.0))
 (ros::_k_brot_p . #f(0.0 0.0))
 (ros::_k_brot_tc . #f(0.1 0.1))
 (ros::_k_run_b . #f(0.0 0.0))
 (ros::_d_run_b . #f(0.0 0.0))
 (ros::_tdfke . #f(0.0 0.0))
 (ros::_tdftc . #f(0.0 0.0))
 (ros::_k_run_x . 0.0)
 (ros::_k_run_y . 0.0)
 (ros::_d_run_x . 0.0)
 (ros::_d_run_y . 0.0)
 (ros::_eefm_k1 . #f(-1.27286 -1.27286))
 (ros::_eefm_k2 . #f(-0.363674 -0.363674))
 (ros::_eefm_k3 . #f(-0.162 -0.162))
 (ros::_eefm_zmp_delay_time_const . #f(0.055 0.055))
 (ros::_eefm_ref_zmp_aux . #f(0.0 0.0))
 (ros::_eefm_rot_damping_gain . #<std_msgs::float64multiarray #X6888080>)
 (ros::_eefm_rot_time_const . #<std_msgs::float64multiarray #X68140d0>)
 (ros::_eefm_pos_damping_gain . #<std_msgs::float64multiarray #X6813980>)
 (ros::_eefm_pos_time_const_support . #<std_msgs::float64multiarray #X6811430>)
 (ros::_eefm_pos_time_const_swing . 0.08)
 (ros::_eefm_pos_transition_time . 0.01)
 (ros::_eefm_pos_margin_time . 0.02)
 (ros::_eefm_leg_inside_margin . 0.062)
 (ros::_eefm_leg_outside_margin . 0.062)
 (ros::_eefm_leg_front_margin . 0.13)
 (ros::_eefm_leg_rear_margin . 0.095)
 (ros::_eefm_body_attitude_control_gain . #f(1.5 1.5))
 (ros::_eefm_body_attitude_control_time_const . #f(10000.0 10000.0))
 (ros::_eefm_cogvel_cutoff_freq . 6.0)
 (ros::_eefm_wrench_alpha_blending . 0.75)
 (ros::_eefm_alpha_cutoff_freq . 1.000000e+07)
 (ros::_eefm_gravitational_acceleration . 9.80665)
 (ros::_eefm_ee_pos_error_p_gain . 0.0)
 (ros::_eefm_ee_rot_error_p_gain . 0.0)
 (ros::_eefm_ee_error_cutoff_freq . 50.0)
 (ros::_is_ik_enable t t nil nil)
 (ros::_st_algorithm . 0)
 (ros::_controller_mode . 1)
 (ros::_transition_time . 2.0)
 (ros::_cop_check_margin . 0.02)
 (ros::_cp_check_margin . 0.06)
 (ros::_contact_decision_threshold . 50.0)
 (ros::_foot_origin_offset . #<std_msgs::float64multiarray #Xa0284c8>)
 (ros::_emergency_check_mode . 0))
nil
27.E4-irteusgl$ (pprint (send (send *ri* :get-auto-balancer-param) :slots))
((plist)
 (ros::_default_zmp_offsets . #<std_msgs::float64multiarray #X6976d38>)
 (ros::_move_base_gain . 0.8)
 (ros::_controller_mode . 1)
 (ros::_graspless_manip_mode)
 (ros::_graspless_manip_arm . "arms")
 (ros::_graspless_manip_p_gain . #f(0.0 0.0 0.0))
 (ros::_graspless_manip_reference_trans_pos . #f(0.0 0.0 0.0))
 (ros::_graspless_manip_reference_trans_rot
  . #f(1.0 0.0 0.0 0.0))
 (ros::_transition_time . 2.0)
 (ros::_zmp_transition_time . 1.0)
 (ros::_adjust_footstep_transition_time . 2.0)
 (ros::_leg_names "rleg" "lleg")
 (ros::_has_ik_failed)
 (ros::_pos_ik_thre . 1.000000e-04)
 (ros::_rot_ik_thre . 0.000175))
nil
28.E4-irteusgl$ (pprint (send (send *ri* :get-gait-generator-param) :slots))
((plist)
 (ros::_default_step_time . 1.1)
 (ros::_default_step_height . 0.05)
 (ros::_default_double_support_ratio . 0.32)
 (ros::_default_double_support_static_ratio . 0.0)
 (ros::_stride_parameter . #f(0.15 0.05 10.0 0.05))
 (ros::_default_orbit_type . 4)
 (ros::_swing_trajectory_delay_time_offset . 0.2)
 (ros::_swing_trajectory_final_distance_weight . 3.0)
 (ros::_stair_trajectory_way_point_offset . #f(0.03 0.0 0.0))
 (ros::_cycloid_delay_kick_point_offset . #f(-0.1 0.0 0.0))
 (ros::_gravitational_acceleration . 9.80665)
 (ros::_toe_pos_offset_x . 0.142869)
 (ros::_heel_pos_offset_x . -0.105784)
 (ros::_toe_zmp_offset_x . 0.079411)
 (ros::_heel_zmp_offset_x . -0.105784)
 (ros::_toe_angle . 0.0)
 (ros::_heel_angle . 0.0)
 (ros::_toe_heel_phase_ratio
  . #f(0.05 0.25 0.2 0.0 0.2 0.25 0.05))
 (ros::_use_toe_joint . t)
 (ros::_use_toe_heel_transition)
 (ros::_zmp_weight_map . #f(1.0 1.0 0.1 0.1))
 (ros::_leg_default_translate_pos . #<std_msgs::float64multiarray #X697eb78>)
 (ros::_optional_go_pos_finalize_footstep_num . 0))
nil
30.E5-irteusgl$ (pprint (send (send *ri* :get-kalman-filter-param) :slots))
((plist)
 (ros::_q_angle . 0.001)
 (ros::_q_rate . 0.003)
 (ros::_r_angle . 1000.0)
 (ros::_kf_algorithm . 0)
 (ros::_acc_offset . #f(0.0 0.0 0.0))
 (ros::_sensorrpy_offset . #f(0.0 0.0 0.0)))
nil

こちらになります.

[otsubo]

ただ，今のパラメータで全く歩けないわけではなく,stを入れるとHRP2が少しずつ傾いていきます.
傾きが小さい間は多少はこけないです.

[snozawa]

うーん、おかしい挙動ですね。
パラメータみてみますが、
start-jsk/rtmros_tutorials#377
はバグなのでgit pullしておいてください。

[snozawa]

(send (send (send *ri* :get-st-param) :eefm_pos_damping_gain) :data)
(send (send (send *ri* :get-st-param) :eefm_rot_damping_gain) :data)
(send (send (send *ri* :get-st-param) :eefm_rot_time_const) :data)
(send (send (send *ri* :get-st-param) :eefm_pos_time_const_support) :data)

の内容もおしえてください。

[snozawa]

そういえばこちらは@sasabotさんが詳しいと思いました。
@sasabotさんの手元では動いてるかな？
(gazeboが新しすぎるかな？)

[otsubo]

20.E2-irteusgl$ (send (send (send *ri* :get-st-param) :eefm_pos_damping_gain) :data)
#f(1.750000e+05 1.750000e+05 3850.0 1.750000e+05 1.750000e+05 3850.0 1.750000e+05 1.750000e+05 3850.0 1.750000e+05 1.750000e+05 3850.0)
21.E2-irteusgl$ (send (send (send *ri* :get-st-param) :eefm_rot_damping_gain) :data)
#f(22.0 22.0 100000.0 22.0 22.0 100000.0 22.0 22.0 100000.0 22.0 22.0 100000.0)
22.E2-irteusgl$ (send (send (send *ri* :get-st-param) :eefm_rot_time_const) :data)
#f(1.5 1.5 1.5 1.5 1.5 1.5 1.5 1.5 1.5 1.5 1.5 1.5)
23.E2-irteusgl$ (send (send (send *ri* :get-st-param) :eefm_pos_time_const_support) :data)
#f(1.5 1.5 1.5 1.5 1.5 1.5 1.5 1.5 1.5 1.5 1.5 1.5)

こちらになります.

[sasabot]

Gazebo4でしか確認していませんが（Gazebo5では動いていない、JADEは5なので5で動いて欲しい）

(send *ri* :start-auto-balancer)
(send *ri* :set-st-param :st-algorithm 1)
(send *ri* :set-st-param :eefm-rot-damping-gain 600 :eefm-pos-damping-gain (float-vector 1.750000e+05 1.750000e+06 (* 4 3850.0)))
(calculate-eefm-st-state-feedback-gain (elt (send *hrp2jsknt* :centroid) 2))
(send *ri* :start-st)

と設定すると、なんとなく新stで歩いていました
また、月の重力だと上の画像みたいにロボットがどんどん傾いていたので、

(send *ri* :calibrate-inertia-sensor)

すると、直っていたような気がします

[YoheiKakiuchi]

#191
これと同じ問題ですか？ HRPのIMUは回転されてモデルに付いているかな？

（Gazebo5では動いていない、JADEは5なので5で動いて欲しい）

動いていないとはどういう挙動ですか？
情報は共有してもらえるとありがたいです。

[otsubo]

@sasabot さん

(send *ri* :set-st-param :eefm-rot-damping-gain 600 :eefm-pos-damping-gain (float-vector 1.750000e+05 1.750000e+06 (* 4 3850.0)))

この行で

6.E1-irteusgl$ (send *ri* :set-st-param :eefm-rot-damping-gain 600 :eefm-pos-damping-gain (float-vector 1.750000e+05 1.750000e+06 (* 4 3\
850.0)))
/home/leus/ros/hydro_parent/devel/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: object expected in (send ros::_eefm_rot_damping\
_gain :serialization-length)
8.E2-irteusgl$ btrace
euserror
euserror
(send ros::_eefm_rot_damping_gain :serialization-length)
(+ (* 8 2) (* 8 2) (* 8 2) (* 8 2) (* 8 2) (* 8 2) (* 8 2) (* 8 2) 8 8 8 8 (* 8 2) (* 8 2) (* 8 2) (* 8 2) (* 8 2) (send ros::_eefm_rot_\
damping_gain :serialization-length) (send ros::_eefm_rot_time_const :serialization-length) (send ros::_eefm_pos_damping_gain :serializat\
ion-length) (send ros::_eefm_pos_time_const_support :serialization-length) 8 8 8 8 8 8 8 (* 8 2) (* 8 2) 8 8 8 8 8 8 8 (* 1 (length ros:\
:_is_ik_enable)) 4 8 8 8 8 8 8 (send ros::_foot_origin_offset :serialization-length) 8)
(send ros::_i_param :serialization-length)
(+ (send ros::_i_param :serialization-length))
(ros::service-call (format nil "/~A/~A" rosbridge-name "setParameter") (instance #<metaclass #X5edb4e8 hrpsys_ros_bridge::openhrp_stabil\
izerservice_setparameterrequest> :init :i_param ros::__i_param))
(or (ros::service-call (format nil "/~A/~A" rosbridge-name "setParameter") (instance #<metaclass #X5edb4e8 hrpsys_ros_bridge::openhrp_st\
abilizerservice_setparameterrequest> :init :i_param ros::__i_param)) (error ";; service call failed (~A)" :stabilizerservice_setparamete\
r))
(send self :stabilizerservice_setparameter :i_param param)
t

このようなエラーが出てしまいます.
また,

(send *ri* :calibrate-inertia-sensor)

を実行したのですが,傾きは残ったままでした.
他にチェックすべきポイントはあるでしょうか.
よろしくお願いいたします.

[snozawa]

そういえば:st-algorithm :tpccにしているので:eefm-xxxパラメータは特に不要でした。
gazeboを起動してたたせただけの状態で、

(send *ri* :state :imucoords)
(send *robot* :imu-sensors)

をおしえてください

[otsubo]

5.E1-irteusgl$ (send *ri* :state :imucoords)
#<coordinates #X899fc98  105.585 -14.574 -1183.528 / -0.014 -0.005 -0.011>
6.E1-irteusgl$ (send *robot* :imu-sensors)
(#<cascaded-coords #X8cc6d10 gyrometer  -98.0 0.0 1186.7 / 0.0 0.0 0.0> #<cascaded-coords #X8cc7fb8 gsensor  -98.0 0.0 1186.7 / 0.0 0.0 \
0.0>)

こちらになります.