Unstable behaviour at startup in the robot using gazebo_ros2_control

[saikishor]

Hello!

We are currently migrating our Humanoids to ROS 2, and in this process we started with gazebo simulation migration. The version of ROS1 is working without any issues, however in ROS 2 the robot acts crazy and flies away. I tried removing the ´gazebo_ros2_control´ and then the robot falls to ground as expected, but as soon as I add gazebo_ros2_control the robot goes crazy. I've attached the videos for your reference:

Without gazebo_ros2_control plugin:

talos_without_control.mp4

With gazebo_ros2_control plugin:

talos_with_control.mp4

Repos to reproduce the issue:

https://github.com/pal-robotics/talos_robot (branch: ros2-migration)
https://github.com/pal-robotics/talos_simulation (branch: ros2-migration)
https://github.com/pal-robotics/talos_moveit_config (branch: ros2-migration)
https://github.com/pal-robotics/launch_pal (branch: master)

- git: {local-name: launch_pal, uri: 'https://github.com/pal-robotics/launch_pal', version: 'master'}
- git: {local-name: talos_moveit_config, uri: 'https://github.com/pal-robotics/talos_moveit_config', version: 'ros2-migration'}
- git: {local-name: talos_robot, uri: 'https://github.com/pal-robotics/talos_robot', version: 'ros2-migration'}
- git: {local-name: talos_simulation, uri: 'https://github.com/pal-robotics/talos_simulation', version: 'ros2-migration'}

Steps to Reproduce:

1. Launch the following command

ros2 launch talos_gazebo talos_gazebo.launch.py

[christophfroehlich]

Are you using the same gazebo engine with ROS 1?

[saikishor]

I'm using the Gazebo classic. I'm not sure about the internal engine, if there is a change. However, it seems weird that only upon using gazebo_ros2_control plugin this is happening.

[saikishor]

@ahcorde Any idea what could be the root cause of this issue? I can introspect if you can give me some pointers.

Thank you :)

[saikishor]

I believe I found the issue, if I use the position interface directly it doesn't work at all. I had to use position with PID to make it work. In past, in our overlayed version of gazebo_ros_control, we were using the PIDs one as well. After finding the issue, I saw somethings that can be improved here, I will open a PR soon to address them.

Thank you for the support

[ahcorde]

are you planning to move this to (new) gazebo, I believe the PID controller is not available there but I can open the PR if you need this feature

[saikishor]

are you planning to move this to (new) gazebo, I believe the PID controller is not available there but I can open the PR if you need this feature

Hello!

I believe we will need it in the new gazebo as well when we start migration to Jazzy. Yes, I'm going to open it here first for your feedback and then, I will open the following PR in the gz_ros2_control

[christophfroehlich]

Then this is the same issue as with #240, the gazebo classic engine does not work well with position interface.
gz_ros2_control is different because it already has a position-controller using velocity interface to the physics engine. @saikishor it could work out of the box for you.

[saikishor]

@christophfroehlich the issue #240 was for passive joints right?. In my case, the active joints have this issue. I see the logic you were talking about https://github.com/ros-controls/gz_ros2_control/blob/4febdcfa8e2bbcf4eaf1107a53495952d3a3ce94/gz_ros2_control/src/gz_system.cpp#L641-L660. Along with the PIDs, if I have the hold_jonits as true without starting any controllers, the robot also behaves as above. In my case, I needed to set hold_joints to false and also have the PID-based position control.

Maybe the gz_ros2_control works out of the box, but we are using Humble internally and we want to check this one first as we also want to make the simulation public. I can try testing with gz_ros2_control in the following weeks. If it works, I would like to change the position_proportional_gain_ to be a vector rather than one for all the joints, because It might be interesting to be able to tune the behaviour also per joint basis.

https://github.com/ros-controls/gz_ros2_control/blob/4febdcfa8e2bbcf4eaf1107a53495952d3a3ce94/gz_ros2_control/src/gz_system.cpp#L203-L237

[christophfroehlich]

Actually, I experienced the same with interacting with objects (with a mimicked gripper), so I guess this is a general limitation of the physics engine.

[christophfroehlich]

Have you tried to only set a proportional gain, like the current gz_ros2_control plugin does? Or do you really need a full PID controller?

[saikishor]

Have you tried to only set a proportional gain, like the current gz_ros2_control plugin does? Or do you really need a full PID controller?

I believe yes, as in this case, we are controlling in EFFORT, and in the gz_ros2_control we are using VELOCITY

[saikishor]

@christophfroehlich the issue #240 was for passive joints right?. In my case, the active joints have this issue. I see the logic you were talking about https://github.com/ros-controls/gz_ros2_control/blob/4febdcfa8e2bbcf4eaf1107a53495952d3a3ce94/gz_ros2_control/src/gz_system.cpp#L641-L660. Along with the PIDs, if I have the hold_jonits as true without starting any controllers, the robot also behaves as above. In my case, I needed to set hold_joints to false and also have the PID-based position control.

Maybe the gz_ros2_control works out of the box, but we are using Humble internally and we want to check this one first as we also want to make the simulation public. I can try testing with gz_ros2_control in the following weeks. If it works, I would like to change the position_proportional_gain_ to be a vector rather than one for all the joints, because It might be interesting to be able to tune the behaviour also per joint basis.

https://github.com/ros-controls/gz_ros2_control/blob/4febdcfa8e2bbcf4eaf1107a53495952d3a3ce94/gz_ros2_control/src/gz_system.cpp#L203-L237

@ahcorde Are you okay with this change?. If you have any comments, I'm happy to hear

[ahcorde]

is this issue already resolve with your PR ?

[saikishor]

You are right!. It's solved. I'll close it. Thanks for reminding