search

pocketxjl / humanoid-control

A humanoid bipedal walking control repo using NMPC and WBC, and simulation with mujoco. Contact: xuejl2001@mail.ustc.edu.cn
MIT License
188 stars 34 forks source link

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

Closed huber-yaoer closed 3 months ago

huber-yaoer commented 3 months ago

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

发现三个消息未编译： 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 3 months ago

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

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 3 months ago

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

huber-yaoer commented 3 months ago

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

huber-yaoer commented 3 months ago

在/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 3 months ago

以上是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本地跑