Servo joint doesn't respect position limits

[peci1]

Bug Report

• I checked the documentation and the forum but found no answer.
• I checked to make sure that this issue has not already been filed.

Environment

• DART version: 6.10 from https://launchpad.net/~openrobotics/+archive/ubuntu/dart (openrobotics fork)
• OS name and version name(or number): Ubuntu 18.04
• Compiler name and version number: GCC 8.4.0

Expected Behavior

Servo motors respect position limits.

Current Behavior

Position limits can be overcome when controlling a revolute joint in SERVO mode. It is apparent that the physics engine slows down the motion behind the position limit, but it does not stop it at the limit.

Steps to Reproduce

1. Create SERVO revolute joint that is in an equilibrium by default (e.g. a link hanging straight down from another link).
2. Set its position limits to the joint.
3. Apply command to the joint that should get it past the position limit.
4. Observe that the joint has actually got past the limits.

Code to Reproduce

I generate the skeleton from SDF (using ign-physics). I haven't worked with SKEL files, so I can't create one, but I assume this scenario would be pretty easy to convert:

    <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 1 0</xyz>
        </axis>
      </joint>
    </model>

This test fails:

  auto *dartBaseLink = skeleton->getBodyNode("base");
  auto *dartJoint = skeleton->getJoint("j1");

  dartJoint->setPositionLowerLimit(0, -0.1);
  dartJoint->setPositionUpperLimit(0, 0.1);
  dartJoint->setLimitEnforcement(true);
  dartJoint->setActuatorType(dart::dynamics::Joint::SERVO);

  for (std::size_t i = 0; i < 1000; ++i)
  {
    dartJoint->setCommand(0, 1);
    dartWorld->step();
  }

  EXPECT_NEAR(0.1, dartJoint->getPosition(0), 1e-2);
  EXPECT_NEAR(0, dartJoint->getVelocity(0), 1e-6);

The joint starts at position 0 with a limit -0.1...0.1 . Then I apply velocity 1 m/s for one second. I'd assume the joint would stop at 0.1 with zero velocity. But this is the actual result:

/media/data/ign/ign-physics/dartsim/src/JointFeatures_TEST.cc:318: Failure
The difference between 0.1 and dartJoint->getPosition(0) is 0.45049999911170779, which exceeds 1e-2, where
0.1 evaluates to 0.10000000000000001,
dartJoint->getPosition(0) evaluates to 0.55049999911170777, and
1e-2 evaluates to 0.01.
/media/data/ign/ign-physics/dartsim/src/JointFeatures_TEST.cc:319: Failure
The difference between 0 and dartJoint->getVelocity(0) is 0.50000002102452545, which exceeds 1e-6, where
0 evaluates to 0,
dartJoint->getVelocity(0) evaluates to 0.50000002102452545, and
1e-6 evaluates to 9.9999999999999995e-07.

I think this bug is even described in this test:

dart/tests/comprehensive/test_Joints.cpp

Lines 1022 to 1025 in 5e128ee

	// TODO(JS): Position limits and servo motor with infinite force limits
	// doesn't work together because they compete against each other to achieve
	// different joint velocities with their infinit force limits. In this case,
	// the position limit constraint should dominent the servo motor constraint.

[jslee02]

Thank you for the detailed report! This should be fixed by #1587

[peci1]

Thanks!

[livanov93]

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

@peci1 @jslee02 I still experience similar problem 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)

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.