Joint Controllers not working properly when joint limits are specified

livanov93 commented 2 years ago

Environment

OS Version:

Distributor ID: Ubuntu
Description:    Ubuntu 22.04.1 LTS
Release:    22.04
Codename:   jammy

Source build - branch ign-gazebo6

Description

Expected behaviour: In Velocity or Force mode joint position limits should be respected and the joints should be able to recover if the commanded velocity results in hitting the position limits.
Actual behavior: When using joints with position limits, which is often the case in robotics, the Velocity mode joint controller can't recover when it hits the limit --> model joint_controller_demo while the Force mode joint controller is stalling - accumulates the error in the integral component (unless we specify manually some small value - absolute values for i_max and i_min ) which means the anti-windup clamping method default parameters are too high - at least there should be some "good guess" explanation for the users of ignition::math::PID and directions on how it works with the transfer function of the joint. The only situation in which Force mode joint controller works without the problem is when the sequential commanded velocities are of similar absolute value with opposite sign.

Steps to reproduce

git clone https://github.com/gazebosim/gz-sim.git
cd gz-sim && mkdir build && cd build && cmake ..
make && sudo make install
bash -c "echo 'export IGN_CONFIG_PATH=/usr/local/share/ignition' >> ~/.bashrc"
source ~/.bashrc

Go to ~/.../gz-sim/examples/worlds/joint_controller.sdf and for model joint_controller_demo and its joint j1 under the <axis> section add following:
```
<limit>
<upper>0.75</upper>
<lower>-0.75</lower>
<effort>100</effort>
<velocity>10</velocity>
</limit>
```
Do the same for joint j1 for model joint_controller_demo_2.
Enforce the changes globally on your system with make && sudo make install.
Run the example ign gazebo joint_controller.sdf -v
Test the Velocity mode joint controller with:
- ign topic -t "/model/joint_controller_demo/joint/j1/cmd_vel" -m ignition.msgs.Double -p "data: -5.0"
- joint j1 of model joint_controller_demo will not move
Test the Force mode joint controller with:
- ign topic -t "/model/joint_controller_demo_2/joint/j1/cmd_vel" -m ignition.msgs.Double -p "data: 5.0"
- then after 2 seconds execute: ign topic -t "/model/joint_controller_demo_2/joint/j1/cmd_vel" -m ignition.msgs.Double -p "data: -0.5"
- joint j1 of model joint_controller_demo_2 will stall and start moving after > 30 seconds

Output

https://user-images.githubusercontent.com/26708124/187866697-2105180b-34f3-4f01-bb76-95c55b530baa.mp4

livanov93 commented 1 year ago

@ahcorde based on your answer I decided to check if everything works. I am using dart version 6.12, and still the joint control in velocity mode fails once the joint reaches position limit. I haven't tested the bullet on garden yet...Have you built everything from source while testing?

DatSpace commented 1 year ago

Not sure if its strictly related, but I had a similar weird issue. When using the component JointVelocityCmd and setting the velocity close to the velocity limit, the command was completely ignored.

Using JointVelocityReset didnt have this issue. Moreover, multiplying with 0.99 solved the issue before sending the command (99% of the velocity limit) but multiplying with 99.9% didn't, implying there might be some rounding error somewhere ?

I would expect that if the command is above the limit that a warning is printed if its ignored, and that even if the command is equal or less to the limit it would be executed. Even potentially capping it to the limit and printing a warning, not just ignoring it.

I am using Ubuntu 22.04, packages from October Humble sync and Garden with ODE physics.

mikramarc commented 9 months ago

Hi, I seem to have a similar problem with joint trajectory controller - if the joint ever reaches the joint limit, it becomes unrecoverable - all the next joint trajectory commands will not move the joint anymore. I can prevent that by setting joint limits in the urdf to e.g. negative value so the model itself will get in self-collision and physically prevent the joint from reaching the limit, but that obviously seems quite hacky. Anyone encountered that as well and figured out a reasonable solution?

My setup: Ubuntu 20.04 ROS2 Humble Plugin for control: name='ign_ros2_control::IgnitionROS2ControlPlugin' filename='ign_ros2_control-system'

azeey commented 9 months ago

I was able to reproduce the problem and it looks like it's a bug in DART. The following patch to the dart/constraint/JointConstraint.cpp on the release-6.13 branch fixes the issue for me.

diff --git a/dart/constraint/JointConstraint.cpp b/dart/constraint/JointConstraint.cpp
index faa072d8f21a..bf032f0ffb2a 100644
--- a/dart/constraint/JointConstraint.cpp
+++ b/dart/constraint/JointConstraint.cpp
@@ -238,7 +238,7 @@ void JointConstraint::update()
         // the position error.

         const double C1 = mErrorAllowance * A1;
-        double bouncing_vel = -std::max(B1, C1) / timeStep;
+        double bouncing_vel = -std::min(B1, C1) / timeStep;
         assert(bouncing_vel >= 0);
         bouncing_vel = std::min(bouncing_vel, mMaxErrorReductionVelocity);

@@ -280,7 +280,7 @@ void JointConstraint::update()
         // the position error.

         const double C2 = mErrorAllowance * A2;
-        double bouncing_vel = -std::min(B2, C2) / timeStep;
+        double bouncing_vel = -std::max(B2, C2) / timeStep;
         assert(bouncing_vel <= 0);
         bouncing_vel = std::max(bouncing_vel, -mMaxErrorReductionVelocity);

Without this change bouncing_vel is always 0 and does not contribute to fixing the constraint violation. When the joint reaches the position limit, the desired velocity change is computed as mDesiredVelocityChange[i] = bouncing_vel - velocities[i];, but since both bouncing_vel and velcoties[i] will be 0 at that point, the desired change is also 0.

azeey commented 9 months ago

https://github.com/dartsim/dart/pull/1774 has been merged upstream. This will be available for Harmonic at some point, but will not be fixed for Fortress and Garden. @j-rivero Can we get a release of dart-6.13?

ufrhaidar commented 4 months ago

Has this been fixed in Harmonic? I'm using ROS2 Humble and updated my gazebo to Harmonic in hope to fix this problem, yet it still exists. I'm using the gz-harmonic=1.0.0-1~jammy and ros-humble-ros-gzharmonic=0.244.12-3jammy apt packages.

azeey commented 4 months ago

Yes, a new version of DART (6.13.1) is now available which should fix this problem, so make sure you've updated DART on your system.

ufrhaidar commented 4 months ago

I have DART(6.13.2) yet still having the same issue.

I am using gz::sim::components::JointVelocityCmd to set a velocity for a specific joint. Joint becomes unresponsive and unrecoverable once it reaches its limits. Getting the same error using JointTrajectoryController.

azeey commented 4 months ago

I tested it locally following the reproduction steps in the PR description (changing ign to gz) and I found that the velocity mode (item (5) in the list) works without any issues. The force mode (item (6)) did not work at first and behaved similar to what was described in (6), but changing the integral gain in the SDF to 0 fixed it for me. Alternatively, setting the i_max and i_min to 0.001 and -0.001 respectively also fixed it. So I think the issue with (6) is integral windup, not an issue with DART.

@ufrhaidar can you try to reproduce the steps above? Or do you have an example that can demonstrate your issue?

ufrhaidar commented 4 months ago

I am not using the JointController plugin, but a similar customized version of it that reads the velocity command from a different source (not a gazebo topic) and follows the same logic as JointController's Velocity mode.

Both plugins set a JointVelocityCmd as the last step in the PreUpdate function. https://github.com/gazebosim/gz-sim/blob/6af74459f4b0f5a2a33fa112e24ac50633a90d3b/src/systems/joint_controller/JointController.cc#L347

I assumed the DART issue was shared between them both (which it is), but it looks like there might be other issues happening after.

@azeey thanks for your help. I'll workaround the issue for now on my end. Will look into the issue furthermore when I have more time.

ufrhaidar commented 4 months ago

@azeey the integral gain issue you mentioned is possibly caused by the PID in gz-math not having an anti-windup. As you reach limit, the integral error keeps increasing.

ufrhaidar commented 3 months ago

@azeey

if you were to use this sdf example.sdf.txt and start the sim

gz sim -v 4 -r example.sdf.txt

Then wait for the joint to reach the limit and command a negative velocity

gz topic -t /model/joint_controller_demo/joint/j1/cmd_vel -m gz.msgs.Double -p "data: -0.1"

does it not get stuck?

@livanov93 @ahcorde any updates on this issue?

https://github.com/gazebosim/gz-sim/assets/110574820/5152a433-28fe-4b0b-9a56-6f51eb337d0c

azeey commented 3 months ago

I was able to reproduce the issue as well. It seems whether it gets stuck at the joint limit depends on the commanded velocity. In the example.sdf.txt file from @ufrhaidar , if we change the <initial_velocity> parameter to 0.2, the joint does not get stuck.

ufrhaidar commented 2 months ago

I was able to reproduce the issue as well. It seems whether it gets stuck at the joint limit depends on the commanded velocity. In the example.sdf.txt file from @ufrhaidar , if we change the <initial_velocity> parameter to 0.2, the joint does not get stuck.

@azeey Yes that was interesting. If you command it before the <initial_velocity> reaches the limit, it works fine, otherwise it gets stuck (being unreliable now after testing it multiple times).

When I was looking at it last time, i noticed that it was getting stuck in the velocity limit bound section in dartsim

https://github.com/dartsim/dart/blob/f9ef83beba401fb1a93bd0727ffc98022dc5f916/dart/constraint/JointConstraint.cpp#L292

gazebosim / gz-sim