Joint can't recover from position limits in servo mode

[livanov93]

Bug Report

• I checked the documentation and the forum but found no answer.
• I found similar issue Servo joint doesn't respect position limits #1583.

Environment

DART version: 6.13 - main branch built from source on commit 189e24d
OS name and version name(or number): Ubuntu 22.04
Compiler name and version number: GCC 11.3.0

Description

I still experience similar problem to #1583 when the axis of rotation is z (e.g. <xyz>0 0 1</xyz>).
The problem is that the joint, once hits the limit and then can't recover from it.

Related issues and PRs:

• Joint Controllers not working properly when joint limits are specified gazebosim/gz-sim#1684
• Mimic Joint Improvement ros-controls/gz_ros2_control#86 (workaround)
• Renamed ign to gz ros-controls/gz_ros2_control#67 (comment) (another perspective)
• Servo joint doesn't respect position limits #1583

I changed the sdf a bit (/home/lovro/tmp/dart/data/sdf/test/test.sdf):

 <sdf version="1.5">
   <model name="simple_joint_test">
        <pose>10 10 2 0  0 0</pose>
        <link name="base">
            <inertial>
                <mass>100</mass>
            </inertial>
            <visual name="visual">
                <pose>0 0 0 -1.57 0 0</pose>
                <geometry>
                    <cylinder>
                        <radius>0.1</radius>
                        <length>0.2</length>
                    </cylinder>
                </geometry>
            </visual>
        </link>
        <link name="bar">
            <pose>0 0 -1 0 0 0</pose>
            <inertial>
                <mass>1</mass>
            </inertial>
            <visual name="visual">
                <geometry>
                    <box>
                        <size>0.1 0.1 0.1</size>
                    </box>
                </geometry>
            </visual>
        </link>
        <joint name="j0" type="fixed">
            <parent>world</parent>
            <child>base</child>
        </joint>
        <joint name="j1" type="revolute">
            <pose>0 0 1 0 0 0</pose>
            <parent>base</parent>
            <child>bar</child>
            <axis>
                <xyz>0 0 1</xyz>
                <limit>
                    <upper>0.85</upper>
                    <lower>-0.75</lower>
                    <effort>100</effort>
                    <velocity>10</velocity>
                </limit>
            </axis>
        </joint>
    </model>
</sdf>

Testing example:

#include <dart/dart.hpp>
#include <dart/utils/sdf/SdfParser.hpp>

using namespace dart;

int main()
{
  // Create a world
  dart::simulation::WorldPtr world(new dart::simulation::World);

  auto sdf_model = dart::utils::SdfParser::readSkeleton(
      "/home/lovro/tmp/dart/data/sdf/test/test.sdf");
  world->addSkeleton(sdf_model);  

  auto skeleton = world->getSkeleton("simple_joint_test");
  auto *dartJoint = skeleton->getJoint("j1");
  
  dartJoint->setLimitEnforcement(true);

  std::cout<<"getPositionLowerLimit = "<<dartJoint->getPositionLowerLimit(0)<<"\n";
  std::cout<<"getPositionUpperLimit = "<<dartJoint->getPositionUpperLimit(0)<<"\n";
  std::cout<<"getForceLowerLimit = "<<dartJoint->getForceLowerLimit(0)<<"\n";
  std::cout<<"getForceUpperLimit = "<<dartJoint->getForceUpperLimit(0)<<"\n";
  std::cout<<"getVelocityUpperLimit = "<<dartJoint->getVelocityUpperLimit(0)<<"\n";
  std::cout<<"getVelocityLowerLimit = "<<dartJoint->getVelocityLowerLimit(0)<<"\n";
  std::cout<<"areLimitsEnforced = "<<dartJoint->areLimitsEnforced()<<"\n";
  std::cout<<"getCoulombFriction = "<<dartJoint->getCoulombFriction(0)<<"\n";
  std::cout<<"world->getGravity() = "<<world->getGravity()<<"\n";
  
  dartJoint->setActuatorType(dart::dynamics::Joint::SERVO);

  
  // move in positive direction
  for (std::size_t i = 0; i < 1000; ++i)
  {
    dartJoint->setCommand(0, 1.0);
    world->step();
  }

  std::cout<<"State after 1000 steps with 1.0 rad/sec: \n";
  std::cout<<dartJoint->getPosition(0)<<"\n";
  std::cout<<dartJoint->getVelocity(0)<<"\n";

  // move in negative direction
  for (std::size_t i = 0; i < 3000; ++i)
  {
    dartJoint->setCommand(0, -1.0);
    world->step();
  }

  std::cout<<"State after 3000 steps with -1.0 rad/sec: \n";
  std::cout<<dartJoint->getPosition(0)<<"\n";
  std::cout<<dartJoint->getVelocity(0)<<"\n";

  return 0;
}

If I change the axis of the revolute joint to <xyz>0 1 0</xyz> or <xyz>1 0 0</xyz> I get good results:

getPositionLowerLimit = -0.75
getPositionUpperLimit = 0.85
getForceLowerLimit = -inf
getForceUpperLimit = inf
getVelocityUpperLimit = inf
getVelocityLowerLimit = -inf
areLimitsEnforced = 1
getCoulombFriction = 0
world->getGravity() =     0
    0
-9.81
State after 1000 steps with 1.0 rad/sec: 
0.85
-2.12946e-14
State after 3000 steps with -1.0 rad/sec: 
-0.75
1.27693e-14

When I use <xyz>0 0 1</xyz> z axis as rotation axis of the joint I get following:

getPositionLowerLimit = -0.75
getPositionUpperLimit = 0.85
getForceLowerLimit = -inf
getForceUpperLimit = inf
getVelocityUpperLimit = inf
getVelocityLowerLimit = -inf
areLimitsEnforced = 1
getCoulombFriction = 0
world->getGravity() =     0
    0
-9.81
State after 1000 steps with 1.0 rad/sec: 
0.85
0
State after 3000 steps with -1.0 rad/sec: 
0.85
0

I also played with changing the gravity direction, and as I move gravity from z to y axis, both of them stop working. If I move gravity to x axis, then the example can't recover from the joint limits on any axis.

[olzhas]

Hello @jslee02!, is anyone working on this? I can start working on this if no one is on it.

[azeey]

Also posted to gazebosim/gz-sim#1684 (comment)

The following patch 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. Since the position limit branch is taken, this also prevents the servo constraints from being activated. Thus, the joint will never recover once it hits the limit.

[jslee02]

Thank you for the fix, @azeey! Feel free to submit a PR when you have a chance. Alternatively, I could make one as well. Either way works for me.

[jslee02]

The CI tests have passed. I'm going to assume that the change doesn't have a negative impact and will resolve this issue, so I'm closing it. Feel free to reopen if necessary. Thanks!