Allow joint to recover after reaching position limits in servo mode

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
gazebo-forks · Dec 13, 2023 · fe0fdca · fe0fdca
1 parent 9f2d607
commit fe0fdca
Showing 1 changed file with 2 additions and 2 deletions.
diff --git 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);