From f82833da28e462129dcc4e4b9ca3b57d1ed3c147 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 13 Dec 2023 16:40:00 -0600 Subject: [PATCH] Allow joint to recover after reaching position limits in servo mode (#1774) Signed-off-by: Addisu Z. Taddese --- dart/constraint/JointConstraint.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dart/constraint/JointConstraint.cpp b/dart/constraint/JointConstraint.cpp index faa072d8f21a9..bf032f0ffb2a1 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);