Robot not stable

[SangliTeng]

Hi Qiayuan:

Thanks for sharing your legged robot controller!

When I run this controller and switch to trotting mode, the robot motion is not stable. The robot also falls when I add a way point command in the rviz.

I wonder if there is a need to tune the parameter or my URDF model is not loaded correctly.

Thank you!

[qiayuanl]

You don't need to tune the parameters of controller, if you setup everything correctly, the robot will be very stable in the simulation. I guess you are using GPU to accelerate the gazebo simulation, you should use a default gazebo.

[SangliTeng]

This is how the simulation works on my desktop. I am following the instruction to install everything.

Shall I run the Gazebo headless to disable the GPU acceleration?

https://user-images.githubusercontent.com/37236559/192186052-4aa607b7-7e27-49d3-83bb-52834c675436.mp4

[qiayuanl]

Shall I run the Gazebo headless to disable the GPU acceleration?

You don't need to run the Gazebo headless.

The simulation result is obviously not correct

[Ke-Wang1017]

Hi @SangliTeng , you don't need to run headless, you just need to disable GPU. After the robot is stable, you can switch on GPU again.

[edward9503]

Hi @WangKeAlchemist @qiayuanliao @SangliTeng ,

I also have the same issue when I ran the trot gait. On my PC, I don't have any Nvidia graphic card. What should I do to solve this problem? To disable GPU, do you mean something like this: https://answers.gazebosim.org/question/21170/how-to-run-gazebo-with-a-specific-graphic-card-or-no-graphic-card/ ?

[edward9503]

Hi all @WangKeAlchemist @qiayuanliao @SangliTeng ,

I think the problem might exist here: if you take a look at the video below, or at the attached video by @SangliTeng, we can see that there is one red curve connected between the base and the ground. https://user-images.githubusercontent.com/39060488/192232499-4354bd58-24b3-4b06-8307-686826e227c2.mp4

As I have worked a lot with the ocs2 before, this is the optimized base trajectory obtained from the MPC, and it should not goes down to the ground. I'm not sure if that is the problem caused by our improper setup or potential bugs in the code.

[qiayuanl]

I have not encountered this problem. @WangKeAlchemist may able to give you the solution, after getting more details, I will fix it.

[edward9503]

@qiayuanliao I don't really think that was the problem caused by GPU because as I mentioned, I only have an integrated graphic card on my PC. But I am also not 100% sure about that. Let's see if someone has a solution for this.

[qiayuanl]

As I have worked a lot with the ocs2 before, this is the optimized base trajectory obtained from the MPC, and it should not goes down to the ground. I'm not sure if that is the problem caused by our improper setup or potential bugs in the code.

The optimized trajectory is very strange indeed.

[edward9503]

As I have worked a lot with the ocs2 before, this is the optimized base trajectory obtained from the MPC, and it should not goes down to the ground. I'm not sure if that is the problem caused by our improper setup or potential bugs in the code.

The optimized trajectory is very strange indeed.

Hi @qiayuanliao , yes, it is weird and we can observe it in my video as well as in @SangliTeng 's video. I think both of us are following the same instructions. Maybe there are hidden bugs somewhere?

[qiayuanl]

As I have worked a lot with the ocs2 before, this is the optimized base trajectory obtained from the MPC, and it should not goes down to the ground. I'm not sure if that is the problem caused by our improper setup or potential bugs in the code.

The optimized trajectory is very strange indeed.

Hi @qiayuanliao , yes, it is weird and we can observe it in my video as well as in @SangliTeng 's video. I think both of us are following the same instructions. Maybe there are hidden bugs somewhere?

Yes,I think there are some bugs, I will keep an eye on this. Could you please trying this on other computers for gathering more information?

[edward9503]

As I have worked a lot with the ocs2 before, this is the optimized base trajectory obtained from the MPC, and it should not goes down to the ground. I'm not sure if that is the problem caused by our improper setup or potential bugs in the code.

The optimized trajectory is very strange indeed.

Hi @qiayuanliao , yes, it is weird and we can observe it in my video as well as in @SangliTeng 's video. I think both of us are following the same instructions. Maybe there are hidden bugs somewhere?

Yes,I think there are some bugs, I will keep an eye on this. Could you please trying this on other computers for gathering more information?

Sure, I will try it and get it back to you.

[edward9503]

Hi @qiayuanliao , I am still trying to find another PC and try the code on it. Btw, I suddenly realize that maybe there is a mistake about the terminal condition for the mpc, because I observe that the terminal position of the base trajectory always locates on the ground. I guess maybe the z-position is set to 0 which causes the problem?

[HariP19]

https://user-images.githubusercontent.com/18585930/192717469-0f830b49-aeef-4270-bb05-93b92b493f46.mp4

Hi! I am facing the same issue with A1 in Gazebo. I tried two different computers, the problem seems to be the same. Even during the stance gait the robot is not fully standing up. I don't think it's a GPU issue because using inbuilt GPU (intel) doesn't seem to solve the issue. Also, I tried running both DDP and SQP, same behavior is observed for both. I am thinking the problem is something to do at the interface level.

[qiayuanl]

Please show me more videos and data, including the optimized base trajectory and different gait. I have never met a problem like this. I believe there are some bugs in the code.

[qiayuanl]

There's possible that the legged_control uses the CppAd files generated by ocs2_legged_robot_ros after running the ocs2_legged_robot_ros. Try delete /tmp/ocs2 on your root or set recompileLibrariesCppAd in legged_control/config/task.info to true.

[qiayuanl]

I fixed it in the latest commit.

[edward9503]

There's possible that the legged_control uses the CppAd files generated by ocs2_legged_robot_ros after running the ocs2_legged_robot_ros. Try delete /tmp/ocs2 on your root or set recompileLibrariesCppAd in legged_control/config/task.info to true.

Thx!

[suihu-mk]

I also encountered the same problem. In addition, sometimes the robot will not stand after starting the controller. After entering the gait, it will lie on the ground to perform the gait. When navigating in rviz, it will stand and move. In this case, the robot is stable when moving. That is, if the robot directly stands up, it will be unstable when moving.

[qiayuanl]

I also encountered the same problem. In addition, sometimes the robot will not stand after starting the controller. After entering the gait, it will lie on the ground to perform the gait. When navigating in rviz, it will stand and move. In this case, the robot is stable when moving. That is, if the robot directly stands up, it will be unstable when moving.

The gait and the goal are separate. So if you only send gait command, the robot will trotting but lie on the ground, this preforming is correct.

Can you try to find out what causes directly standing up without goal command?

[HariP19]

@suihu-mk You can run the following to make the robot standup via terminal before starting trotting to make the robot standup(for A1) rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{header: {stamp: now, frame_id: "odom"}, pose: {position: {x: 0.0, y: 0.0, z: 3.0}, orientation: {w: 1.0}}}'

@qiayuanliao Thanks! It works properly after the last commit. Also ocs2 has a nice terminal interface for setting target (legged_robot_target node in ocs2_legged_robot_ros pkg), perhaps you can add it to your read me to make it easier for users? Just a small suggestion :)

[edward9503]

@suihu-mk You can run the following to make the robot standup via terminal before starting trotting to make the robot standup(for A1) rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{header: {stamp: now, frame_id: "odom"}, pose: {position: {x: 0.0, y: 0.0, z: 3.0}, orientation: {w: 1.0}}}'

@qiayuanliao Thanks! It works properly after the last commit. Also ocs2 has a nice terminal interface for setting target (legged_robot_target node in ocs2_legged_robot_ros pkg), perhaps you can add it to your read me to make it easier for users? Just a small suggestion :)

That's a good suggestion. Personally, I also think typing the desired position in a terminal should be more convenient.

[qiayuanl]

@suihu-mk You can run the following to make the robot standup via terminal before starting trotting to make the robot standup(for A1) rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{header: {stamp: now, frame_id: "odom"}, pose: {position: {x: 0.0, y: 0.0, z: 3.0}, orientation: {w: 1.0}}}'

@qiayuanliao Thanks! It works properly after the last commit. Also ocs2 has a nice terminal interface for setting target (legged_robot_target node in ocs2_legged_robot_ros pkg), perhaps you can add it to your read me to make it easier for users? Just a small suggestion :)

That's a good suggestion. Personally, I also think typing the desired position in a terminal should be more convenient.

Initially I use legged_robot_target node, but If you are experimenting with real hardware, it's relatively dangerous since you may don't know the current position and orientation before type a command.

[edward9503]

@suihu-mk You can run the following to make the robot standup via terminal before starting trotting to make the robot standup(for A1) rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{header: {stamp: now, frame_id: "odom"}, pose: {position: {x: 0.0, y: 0.0, z: 3.0}, orientation: {w: 1.0}}}' @qiayuanliao Thanks! It works properly after the last commit. Also ocs2 has a nice terminal interface for setting target (legged_robot_target node in ocs2_legged_robot_ros pkg), perhaps you can add it to your read me to make it easier for users? Just a small suggestion :)

That's a good suggestion. Personally, I also think typing the desired position in a terminal should be more convenient.

Initially I use legged_robot_target node, but If you are experimenting with real hardware, it's relatively dangerous since you may don't know the current position and orientation before type a command.

This node only requires a relative position and orientation command to the current position and orientation, so it should be fine.

Btw, how should we set the desired target position and orientation in hardware? Do we also need to use the target arrow in Rviz for the setting?

[qiayuanl]

@suihu-mk You can run the following to make the robot standup via terminal before starting trotting to make the robot standup(for A1) rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{header: {stamp: now, frame_id: "odom"}, pose: {position: {x: 0.0, y: 0.0, z: 3.0}, orientation: {w: 1.0}}}' @qiayuanliao Thanks! It works properly after the last commit. Also ocs2 has a nice terminal interface for setting target (legged_robot_target node in ocs2_legged_robot_ros pkg), perhaps you can add it to your read me to make it easier for users? Just a small suggestion :)

That's a good suggestion. Personally, I also think typing the desired position in a terminal should be more convenient.

Initially I use legged_robot_target node, but If you are experimenting with real hardware, it's relatively dangerous since you may don't know the current position and orientation before type a command.

This node only requires a relative position and orientation command to the current position and orientation, so it should be fine.

Btw, how should we set the desired target position and orientation in hardware? Do we also need to use the target arrow in Rviz for the setting?

In hardware, everything will be the same as simulation expected we need to launch the unitree_hw instead of Gazebo.

However, both two trajectory publisher node are some demo test node. In another project, I wrote a node which received path from the navigation stack and convert it to a trajectory.

Anyway, you can implement your own gait and trajectory publisher with few line and very simple code, which might allow you control the robot with joystick or keyboard.

[qiayuanl]

@SangliTeng @edward9503 Has this issue been resolved?

[edward9503]

@suihu-mk You can run the following to make the robot standup via terminal before starting trotting to make the robot standup(for A1) rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{header: {stamp: now, frame_id: "odom"}, pose: {position: {x: 0.0, y: 0.0, z: 3.0}, orientation: {w: 1.0}}}' @qiayuanliao Thanks! It works properly after the last commit. Also ocs2 has a nice terminal interface for setting target (legged_robot_target node in ocs2_legged_robot_ros pkg), perhaps you can add it to your read me to make it easier for users? Just a small suggestion :)

That's a good suggestion. Personally, I also think typing the desired position in a terminal should be more convenient.

Initially I use legged_robot_target node, but If you are experimenting with real hardware, it's relatively dangerous since you may don't know the current position and orientation before type a command.

This node only requires a relative position and orientation command to the current position and orientation, so it should be fine. Btw, how should we set the desired target position and orientation in hardware? Do we also need to use the target arrow in Rviz for the setting?

In hardware, everything will be the same as simulation expected we need to launch the unitree_hw instead of Gazebo.

However, both two trajectory publisher node are some demo test node. In another project, I wrote a node which received path from the navigation stack and convert it to a trajectory.

Anyway, you can implement your own gait and trajectory publisher with few line and very simple code, which might allow you control the robot with joystick or keyboard.

Yes. Overall, thanks for sharing the code. I think this issue has been resolved.

[edward9503]

I also encountered the same problem. In addition, sometimes the robot will not stand after starting the controller. After entering the gait, it will lie on the ground to perform the gait. When navigating in rviz, it will stand and move. In this case, the robot is stable when moving. That is, if the robot directly stands up, it will be unstable when moving.

@qiayuanliao , I think he/she is mentioning this behavior:

https://user-images.githubusercontent.com/39060488/192804926-9af65394-16dd-4b82-81a1-ff766e7433bb.mp4

By using the command mentioned by @HariP19 , it can stand up. Is there any way we can achieve the standing up directly right after we start the controller in the controller manager?

[qiayuanl]

I also encountered the same problem. In addition, sometimes the robot will not stand after starting the controller. After entering the gait, it will lie on the ground to perform the gait. When navigating in rviz, it will stand and move. In this case, the robot is stable when moving. That is, if the robot directly stands up, it will be unstable when moving.

@qiayuanliao , I think he/she is mentioning this behavior:

Screencast.2022-09-28.22.22.17.mp4 By using the command mentioned by @HariP19 , it can stand up. Is there any way we can achieve the standing up directly right after we start the controller in the controller manager?

I believe that he/she is describing the difference between two situation caused by the issue that has been solved.

As I mentioned, THE GAIT AND GOAL ARE COMPLETELY DIFFERENT AND SEPARATED!!! You don't need to type stance while the robot is lying on the ground with four foot touching the ground; it's completely wrong since the robot is already in the stance gait.

By the way, If you want the robot stance up right after the controller starts, you can add the code behind here

current_observation_.state (8) = 0.3;

But it's HIGHLY NO RECOMMENDED and VERY DANGER, and I will never do that.

Again, the target_trajectories_publisher is for demonstration. You can get the feature you mentioned by simply combining the trajectory publisher and gait command into a very simple node and replacing the target_trajectories_publisher; you can even add gamepad and keyboard input for different gaits and different stance heights and to start/stop controller (by ros service). I am sorry that I will not implement it for you since everyone has their own demand, lol.

[edward9503]

Thanks for the explanation.

[SangliTeng]

Thank you! The problem is solved now!