Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Joint can't recover from position limits in servo mode #1683

Closed
2 tasks done
livanov93 opened this issue Nov 9, 2022 · 4 comments · Fixed by #1774 · May be fixed by #1818
Closed
2 tasks done

Joint can't recover from position limits in servo mode #1683

livanov93 opened this issue Nov 9, 2022 · 4 comments · Fixed by #1774 · May be fixed by #1818
Labels
type: bug Indicates an unexpected problem or unintended behavior

Comments

@livanov93
Copy link

livanov93 commented Nov 9, 2022 •
edited
Loading

Bug Report

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:

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.
@livanov93 livanov93 added the type: bug Indicates an unexpected problem or unintended behavior label Nov 9, 2022
@olzhas
Copy link
Contributor

olzhas commented Nov 20, 2022 •
edited
Loading

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

@azeey
Copy link
Contributor

azeey commented Dec 13, 2023

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
Copy link
Member

jslee02 commented Dec 13, 2023

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.

@azeey azeey mentioned this issue Dec 13, 2023
Merged
6 tasks
@jslee02
Copy link
Member

jslee02 commented Dec 14, 2023

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!

@jslee02 jslee02 closed this as completed Dec 14, 2023
@azeey azeey mentioned this issue Feb 21, 2024
Open
@ufrhaidar ufrhaidar mentioned this issue Jun 26, 2024
Open
7 tasks
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
type: bug Indicates an unexpected problem or unintended behavior
Projects
None yet
Milestone
No milestone
4 participants
@azeey @olzhas @jslee02 @livanov93