Joint Controllers not working properly when joint limits are specified

[livanov93]

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

2. 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.

3. Enforce the changes globally on your system with make && sudo make install.
4. Run the example ign gazebo joint_controller.sdf -v
5. 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
6. 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

joint_controller.mp4

[livanov93]

@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]

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]

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]

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]

dartsim/dart#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?