pocketxjl/humanoid-control

启动（load_cheat_controller.launch、load_normal_controller.launch），加载完so文件后， 再mujoco窗口按了space之后，teleop输入w之后，机器人stance步态不动trot步态直接倒下

Closed this issue · 5 comments

huber-yaoer commented

大佬，我发现前面没解决的类似问题，补充细节如下：

发现三个消息未编译：
root@5b7bceea6e86:/# rostopic echo /humanoid_mpc_mode_schedule
ERROR: Cannot load message class for [ocs2_msgs/mode_schedule]. Are your messages built?
root@5b7bceea6e86:/# rostopic echo /humanoid_mpc_observation
ERROR: Cannot load message class for [ocs2_msgs/mpc_observation]. Are your messages built?
root@5b7bceea6e86:/# rostopic echo /humanoid_mpc_target
ERROR: Cannot load message class for [ocs2_msgs/mpc_target_trajectories]. Are your messages built?

已执行 catkin build ocs2_msgs 编译，但问题仍然没解决

rosnode图如下：
1717505870107

经排查stance步态，/cmd_vel 、/hummanold_mpc_target 两个主题的消息内容已会根据按w键变动，/normal_controller节点发给/humanold_sim节点的/targetVel、/targetTorque、/targetPos、/targetKp、/targetKd 几个主题消息不会根据按w键变动， 我自己猜测可能是normal_controller节点的问题

trot步态有类似情况，因为倒下的过程各种情况一直在变，不好准确记录，trot步态有类似情况这个事实仅供参考

另外我roslaunch humanoid_dummy legged_robot_sqp.launch是成功运行的

########################################################################

MPC Benchmarking
Maximum : 28.2681[ms].
Average : 3.82532[ms].
########################################################################

WBC Benchmarking
Maximum : 9.66731[ms].
Average : 0.336451[ms]

ros版本：noetic

root@5b7bceea6e86:/humanoid-control# lsb_release -a
No LSB modules are available.
Distributor ID: Ubuntu
Description: Ubuntu 20.04.6 LTS
Release: 20.04
Codename: focal

root@5b7bceea6e86:/humanoid-control# pip show mujoco
Name: mujoco
Version: 3.1.5
Summary: MuJoCo Physics Simulator
Home-page: None
Author: None
Author-email: Google DeepMind mujoco@deepmind.com
License: Apache License 2.0
Location: /usr/local/lib/python3.8/dist-packages
Requires: numpy, absl-py, pyopengl, etils, glfw
Required-by:

huber-yaoer commented

root@5b7bceea6e86:/humanoid-control# roslaunch humanoid_controllers load_normal_controller.launch
... logging to /root/.ros/log/f87f0dec-22ed-11ef-bdd7-0242ac110002/roslaunch-5b7bceea6e86-8785.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://5b7bceea6e86:34123/

SUMMARY

PARAMETERS

  • /gaitCommandFile: /humanoid-control...
  • /humanoid_description: <?xml version="1....
  • /referenceFile: /humanoid-control...
  • /rosdistro: noetic
  • /rosversion: 1.16.0
  • /taskFile: /humanoid-control...
  • /urdfFile: /humanoid-control...
  • /urdfFileOrigin: /humanoid-control...

NODES
/
humanoid_sim (mujoco_sim/humanoid_sim.py)
humanoid_target (humanoid_controllers/humanoid_target_trajectories_publisher)
normal_controller (humanoid_controllers/normal_controller_node)

auto-starting new master
process[master]: started with pid [8793]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to f87f0dec-22ed-11ef-bdd7-0242ac110002
process[rosout-1]: started with pid [8803]
started core service [/rosout]
process[humanoid_target-2]: started with pid [8810]
process[normal_controller-3]: started with pid [8811]
process[humanoid_sim-4]: started with pid [8812]
[ INFO] [1717559094.859033790]: The TargetTrajectories is publishing on humanoid_mpc_target topic.
[HumanoidInterface] Loading task file: "/humanoid-control/src/humanoid-control/humanoid_interface/config/mpc/task.info"
[HumanoidInterface] Loading Pinocchio model from: "/humanoid-control/src/humanoid-control/humanoid-legged-description/urdf/humanoid_legged_control.urdf"
[HumanoidInterface] Loading target command settings from: "/humanoid-control/src/humanoid-control/humanoid_interface/config/command/reference.info"
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/dynamics_systemFlowMap/cppad_generated/dynamics_systemFlowMap_libcppadcg_tmp2768811.so
[CppAdInterface] Renaming /tmp/ocs2/dynamics_systemFlowMap/cppad_generated/dynamics_systemFlowMap_libcppadcg_tmp2768811.so to /tmp/ocs2/dynamics_systemFlowMap/cppad_generated/dynamics_systemFlowMap_lib.so
WARNING: Loaded at least one default value in matrix: "Q"
WARNING: Loaded at least one default value in matrix: "R"
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/l_foot_toe_position/cppad_generated/l_foot_toe_position_libcppadcg_tmp8658811.so
libGL error: MESA-LOADER: failed to retrieve device information
libGL error: MESA-LOADER: failed to open amdgpu: /usr/lib/dri/amdgpu_dri.so: cannot open shared object file: No such file or directory (search paths /usr/lib/x86_64-linux-gnu/dri:$${ORIGIN}/dri:/usr/lib/dri, suffix _dri)
libGL error: failed to load driver: amdgpu
libGL error: failed to open /dev/dri/card0: No such file or directory
libGL error: failed to load driver: radeonsi
[CppAdInterface] Renaming /tmp/ocs2/l_foot_toe_position/cppad_generated/l_foot_toe_position_libcppadcg_tmp8658811.so to /tmp/ocs2/l_foot_toe_position/cppad_generated/l_foot_toe_position_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/l_foot_toe_velocity/cppad_generated/l_foot_toe_velocity_libcppadcg_tmp3578811.so
[CppAdInterface] Renaming /tmp/ocs2/l_foot_toe_velocity/cppad_generated/l_foot_toe_velocity_libcppadcg_tmp3578811.so to /tmp/ocs2/l_foot_toe_velocity/cppad_generated/l_foot_toe_velocity_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/l_foot_toe_orientation/cppad_generated/l_foot_toe_orientation_libcppadcg_tmp3748811.so
[CppAdInterface] Renaming /tmp/ocs2/l_foot_toe_orientation/cppad_generated/l_foot_toe_orientation_libcppadcg_tmp3748811.so to /tmp/ocs2/l_foot_toe_orientation/cppad_generated/l_foot_toe_orientation_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/r_foot_toe_position/cppad_generated/r_foot_toe_position_libcppadcg_tmp7788811.so
[CppAdInterface] Renaming /tmp/ocs2/r_foot_toe_position/cppad_generated/r_foot_toe_position_libcppadcg_tmp7788811.so to /tmp/ocs2/r_foot_toe_position/cppad_generated/r_foot_toe_position_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/r_foot_toe_velocity/cppad_generated/r_foot_toe_velocity_libcppadcg_tmp4288811.so
[CppAdInterface] Renaming /tmp/ocs2/r_foot_toe_velocity/cppad_generated/r_foot_toe_velocity_libcppadcg_tmp4288811.so to /tmp/ocs2/r_foot_toe_velocity/cppad_generated/r_foot_toe_velocity_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/r_foot_toe_orientation/cppad_generated/r_foot_toe_orientation_libcppadcg_tmp4008811.so
[CppAdInterface] Renaming /tmp/ocs2/r_foot_toe_orientation/cppad_generated/r_foot_toe_orientation_libcppadcg_tmp4008811.so to /tmp/ocs2/r_foot_toe_orientation/cppad_generated/r_foot_toe_orientation_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/l_foot_heel_position/cppad_generated/l_foot_heel_position_libcppadcg_tmp2558811.so
[CppAdInterface] Renaming /tmp/ocs2/l_foot_heel_position/cppad_generated/l_foot_heel_position_libcppadcg_tmp2558811.so to /tmp/ocs2/l_foot_heel_position/cppad_generated/l_foot_heel_position_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/l_foot_heel_velocity/cppad_generated/l_foot_heel_velocity_libcppadcg_tmp4328811.so
[CppAdInterface] Renaming /tmp/ocs2/l_foot_heel_velocity/cppad_generated/l_foot_heel_velocity_libcppadcg_tmp4328811.so to /tmp/ocs2/l_foot_heel_velocity/cppad_generated/l_foot_heel_velocity_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/l_foot_heel_orientation/cppad_generated/l_foot_heel_orientation_libcppadcg_tmp7858811.so
[CppAdInterface] Renaming /tmp/ocs2/l_foot_heel_orientation/cppad_generated/l_foot_heel_orientation_libcppadcg_tmp7858811.so to /tmp/ocs2/l_foot_heel_orientation/cppad_generated/l_foot_heel_orientation_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/r_foot_heel_position/cppad_generated/r_foot_heel_position_libcppadcg_tmp358811.so
[CppAdInterface] Renaming /tmp/ocs2/r_foot_heel_position/cppad_generated/r_foot_heel_position_libcppadcg_tmp358811.so to /tmp/ocs2/r_foot_heel_position/cppad_generated/r_foot_heel_position_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/r_foot_heel_velocity/cppad_generated/r_foot_heel_velocity_libcppadcg_tmp4908811.so
[CppAdInterface] Renaming /tmp/ocs2/r_foot_heel_velocity/cppad_generated/r_foot_heel_velocity_libcppadcg_tmp4908811.so to /tmp/ocs2/r_foot_heel_velocity/cppad_generated/r_foot_heel_velocity_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/r_foot_heel_orientation/cppad_generated/r_foot_heel_orientation_libcppadcg_tmp3928811.so
[CppAdInterface] Renaming /tmp/ocs2/r_foot_heel_orientation/cppad_generated/r_foot_heel_orientation_libcppadcg_tmp3928811.so to /tmp/ocs2/r_foot_heel_orientation/cppad_generated/r_foot_heel_orientation_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/dynamics_systemFlowMap/cppad_generated/dynamics_systemFlowMap_libcppadcg_tmp1628811.so
[CppAdInterface] Renaming /tmp/ocs2/dynamics_systemFlowMap/cppad_generated/dynamics_systemFlowMap_libcppadcg_tmp1628811.so to /tmp/ocs2/dynamics_systemFlowMap/cppad_generated/dynamics_systemFlowMap_lib.so
WARNING: Loaded at least one default value in matrix: "Q"
WARNING: Loaded at least one default value in matrix: "R"
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/l_foot_toe_position/cppad_generated/l_foot_toe_position_libcppadcg_tmp3058811.so
[CppAdInterface] Renaming /tmp/ocs2/l_foot_toe_position/cppad_generated/l_foot_toe_position_libcppadcg_tmp3058811.so to /tmp/ocs2/l_foot_toe_position/cppad_generated/l_foot_toe_position_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/l_foot_toe_velocity/cppad_generated/l_foot_toe_velocity_libcppadcg_tmp5148811.so
[CppAdInterface] Renaming /tmp/ocs2/l_foot_toe_velocity/cppad_generated/l_foot_toe_velocity_libcppadcg_tmp5148811.so to /tmp/ocs2/l_foot_toe_velocity/cppad_generated/l_foot_toe_velocity_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/l_foot_toe_orientation/cppad_generated/l_foot_toe_orientation_libcppadcg_tmp5168811.so
[CppAdInterface] Renaming /tmp/ocs2/l_foot_toe_orientation/cppad_generated/l_foot_toe_orientation_libcppadcg_tmp5168811.so to /tmp/ocs2/l_foot_toe_orientation/cppad_generated/l_foot_toe_orientation_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/r_foot_toe_position/cppad_generated/r_foot_toe_position_libcppadcg_tmp9328811.so
[CppAdInterface] Renaming /tmp/ocs2/r_foot_toe_position/cppad_generated/r_foot_toe_position_libcppadcg_tmp9328811.so to /tmp/ocs2/r_foot_toe_position/cppad_generated/r_foot_toe_position_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/r_foot_toe_velocity/cppad_generated/r_foot_toe_velocity_libcppadcg_tmp2368811.so
[CppAdInterface] Renaming /tmp/ocs2/r_foot_toe_velocity/cppad_generated/r_foot_toe_velocity_libcppadcg_tmp2368811.so to /tmp/ocs2/r_foot_toe_velocity/cppad_generated/r_foot_toe_velocity_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/r_foot_toe_orientation/cppad_generated/r_foot_toe_orientation_libcppadcg_tmp1088811.so
[CppAdInterface] Renaming /tmp/ocs2/r_foot_toe_orientation/cppad_generated/r_foot_toe_orientation_libcppadcg_tmp1088811.so to /tmp/ocs2/r_foot_toe_orientation/cppad_generated/r_foot_toe_orientation_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/l_foot_heel_position/cppad_generated/l_foot_heel_position_libcppadcg_tmp4538811.so
[CppAdInterface] Renaming /tmp/ocs2/l_foot_heel_position/cppad_generated/l_foot_heel_position_libcppadcg_tmp4538811.so to /tmp/ocs2/l_foot_heel_position/cppad_generated/l_foot_heel_position_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/l_foot_heel_velocity/cppad_generated/l_foot_heel_velocity_libcppadcg_tmp208811.so
[CppAdInterface] Renaming /tmp/ocs2/l_foot_heel_velocity/cppad_generated/l_foot_heel_velocity_libcppadcg_tmp208811.so to /tmp/ocs2/l_foot_heel_velocity/cppad_generated/l_foot_heel_velocity_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/l_foot_heel_orientation/cppad_generated/l_foot_heel_orientation_libcppadcg_tmp5438811.so
[CppAdInterface] Renaming /tmp/ocs2/l_foot_heel_orientation/cppad_generated/l_foot_heel_orientation_libcppadcg_tmp5438811.so to /tmp/ocs2/l_foot_heel_orientation/cppad_generated/l_foot_heel_orientation_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/r_foot_heel_position/cppad_generated/r_foot_heel_position_libcppadcg_tmp4228811.so
[CppAdInterface] Renaming /tmp/ocs2/r_foot_heel_position/cppad_generated/r_foot_heel_position_libcppadcg_tmp4228811.so to /tmp/ocs2/r_foot_heel_position/cppad_generated/r_foot_heel_position_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/r_foot_heel_velocity/cppad_generated/r_foot_heel_velocity_libcppadcg_tmp8878811.so
[CppAdInterface] Renaming /tmp/ocs2/r_foot_heel_velocity/cppad_generated/r_foot_heel_velocity_libcppadcg_tmp8878811.so to /tmp/ocs2/r_foot_heel_velocity/cppad_generated/r_foot_heel_velocity_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/r_foot_heel_orientation/cppad_generated/r_foot_heel_orientation_libcppadcg_tmp8998811.so
[CppAdInterface] Renaming /tmp/ocs2/r_foot_heel_orientation/cppad_generated/r_foot_heel_orientation_libcppadcg_tmp8998811.so to /tmp/ocs2/r_foot_heel_orientation/cppad_generated/r_foot_heel_orientation_lib.so
WARNING: Failed to set threads priority (one possible reason could be that the user and the group permissions are not set properly.)
WARNING: Failed to set threads priority (one possible reason could be that the user and the group permissions are not set properly.)
WARNING: Failed to set threads priority (one possible reason could be that the user and the group permissions are not set properly.)
WARNING: Failed to set threads priority (one possible reason could be that the user and the group permissions are not set properly.)
[ INFO] [1717559110.343887221]: Waiting for the initial policy ...
[ INFO] [1717559110.351963935]: Initial policy has been received.
pause_flag: 0

<node pkg="humanoid_dummy" type="humanoid_gait_command" name="humanoid_gait_command"

和

<node pkg="mujoco_sim" type="teleop.py" name="teleop"

我是另外的窗口，手动启动的

pocketxjl commented

看起来你是在docker里面跑的仿真？其中mujoco运行是否流畅，是否能保证实时性（即1ms内完成一个步长的仿真运算）

huber-yaoer commented

是在docker内运行的仿真，mujoco运行感觉是流畅的，1ms内完成一个步长的仿真运算我不确定，下午验证一下，roslaunch humanoid_dummy legged_robot_sqp.launch是运行的很流畅没啥毛病

huber-yaoer commented

在/humanoid-control/src/humanoid-control/mujoco_sim/script/humanoid_sim.py的simulate函数内测试
test111 = time.time()
mj.mj_step(self.model, self.data)
print('**********111', (time.time() - test111)*1000, (time.time() - test111)<=1/1000)

  test222 = time.time()
  viewport_width, viewport_height = glfw.get_framebuffer_size(self.window)
  viewport = mj.MjrRect(0, 0, viewport_width, viewport_height)
  mj.mjv_updateScene(self.model, self.data, self.opt, None, self.cam, mj.mjtCatBit.mjCAT_ALL.value, self.scene)
  mj.mjr_render(viewport, self.scene, self.context)
  print('**********222', (time.time() - test222)*1000, (time.time() - test222)<=1/1000)

以上代码运行结果如下，可知mj.mj_step(self.model, self.data)在1ms内，
viewport_width, viewport_height = glfw.get_framebuffer_size(self.window)
viewport = mj.MjrRect(0, 0, viewport_width, viewport_height)
mj.mjv_updateScene(self.model, self.data, self.opt, None, self.cam,mj.mjtCatBit.mjCAT_ALL.value, self.scene)
mj.mjr_render(viewport, self.scene, self.context)
四行代码花费320ms到350ms之间

[ INFO] [1717572999.499661942]: Waiting for the initial policy ...
[ INFO] [1717572999.507033003]: Initial policy has been received.
**********222 325.9584903717041 False
**********222 324.8932361602783 False
**********222 328.3970355987549 False
**********222 327.57019996643066 False
**********222 326.89857482910156 False
**********222 333.0819606781006 False
**********222 319.01049613952637 False
**********222 327.08120346069336 False
**********222 321.2170600891113 False
**********222 335.2985382080078 False
**********222 322.76010513305664 False
**********222 332.7188491821289 False
pause_flag: 0
**********111 0.05030632019042969 True
**********111 0.12946128845214844 True
**********111 0.09894371032714844 True
**********111 0.08368492126464844 True
**********111 0.049114227294921875 True
**********111 0.14638900756835938 True
**********111 0.057220458984375 True
**********111 0.11539459228515625 True
**********111 0.09179115295410156 True
**********111 0.09036064147949219 True
**********111 0.1270771026611328 True
**********111 0.10895729064941406 True
**********111 0.1819133758544922 True
**********111 0.08797645568847656 True
**********111 0.20813941955566406 True
**********111 0.21505355834960938 True
**********111 0.08034706115722656 True
**********222 334.62977409362793 False
**********111 0.16641616821289062 True
**********111 0.12493133544921875 True
**********111 0.12636184692382812 True
**********111 0.080108642578125 True
**********111 0.1308917999267578 True
**********111 0.07963180541992188 True
**********111 0.26035308837890625 True
**********111 0.11134147644042969 True
**********111 0.1456737518310547 True
**********111 0.08153915405273438 True
**********111 0.10633468627929688 True
**********111 0.2541542053222656 True
**********111 0.07843971252441406 True
**********111 0.11944770812988281 True
**********111 0.08463859558105469 True
**********111 0.1327991485595703 True
**********111 0.07367134094238281 True
**********222 341.63427352905273 False
**********111 0.2129077911376953 True
**********111 0.17976760864257812 True
**********111 0.14829635620117188 True
**********111 0.17118453979492188 True
**********111 0.11444091796875 True
**********111 0.19550323486328125 True
**********111 0.13375282287597656 True
**********111 0.09822845458984375 True
**********111 0.16617774963378906 True
**********111 0.09083747863769531 True
**********111 0.15592575073242188 True
**********111 0.09059906005859375 True
**********111 0.20623207092285156 True
**********111 0.09560585021972656 True
**********111 0.14138221740722656 True
**********111 0.10251998901367188 True
**********111 0.08821487426757812 True
**********222 334.4225883483887 False
**********111 0.1633167266845703 True
**********111 0.18668174743652344 True
**********111 0.11348724365234375 True
**********111 0.11157989501953125 True
**********111 0.2262592315673828 True
**********111 0.1049041748046875 True
**********111 0.18286705017089844 True
**********111 0.17070770263671875 True
**********111 0.18286705017089844 True
**********111 0.12350082397460938 True
**********111 0.1690387725830078 True
**********111 0.44417381286621094 True
**********111 0.1373291015625 True
**********111 0.11348724365234375 True
**********111 0.11157989501953125 True
**********111 0.21457672119140625 True
**********111 0.1633167266845703 True
**********222 332.13210105895996 False
**********111 0.1735687255859375 True
**********111 0.1544952392578125 True
**********111 0.09822845458984375 True
**********111 0.08916854858398438 True
**********111 0.09393692016601562 True
**********111 0.08106231689453125 True
**********111 0.09465217590332031 True
**********111 0.09584426879882812 True
**********111 0.14495849609375 True
**********111 0.1583099365234375 True
**********111 0.1780986785888672 True
**********111 0.1277923583984375 True
**********111 0.08320808410644531 True
**********111 0.2129077911376953 True
**********111 0.08296966552734375 True
**********111 0.18024444580078125 True
**********111 0.09036064147949219 True

huber-yaoer commented

以上是ubuntu22.04的电脑，换了个ubuntu20.04的电脑，本地安装了ros1，没用docker，本地跑的此项目，一键启动的，跑通了，没用到GPU，
viewport_width, viewport_height = glfw.get_framebuffer_size(self.window)
viewport = mj.MjrRect(0, 0, viewport_width, viewport_height)
mj.mjv_updateScene(self.model, self.data, self.opt, None, self.cam,mj.mjtCatBit.mjCAT_ALL.value, self.scene)
mj.mjr_render(viewport, self.scene, self.context)
四行代码花费在1ms以内了，ubuntu22.04的电脑可以尝试源码安装ros1本地跑