From cbcb4e58888a23aa64822a01d7bd4d399c60b489 Mon Sep 17 00:00:00 2001 From: Haidar Obeid <110574820+ufrhaidar@users.noreply.github.com> Date: Wed, 26 Jun 2024 17:50:58 +1000 Subject: [PATCH 1/3] fix joint stuck at limit bug --- dart/constraint/JointConstraint.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/dart/constraint/JointConstraint.cpp b/dart/constraint/JointConstraint.cpp index 32ebec57fdfc1..b0fcf91a584cc 100644 --- a/dart/constraint/JointConstraint.cpp +++ b/dart/constraint/JointConstraint.cpp @@ -213,7 +213,7 @@ void JointConstraint::update() const double B1 = A1 + mErrorAllowance; const double A2 = positions[i] - positionUpperLimits[i]; const double B2 = A2 - mErrorAllowance; - if (B1 < 0) { + if (B1 <= 0) { // The current position is lower than the lower bound. // // pos LB UB @@ -234,7 +234,7 @@ void JointConstraint::update() assert(bouncing_vel >= 0); bouncing_vel = std::min(bouncing_vel, mMaxErrorReductionVelocity); - mDesiredVelocityChange[i] = bouncing_vel - velocities[i]; + mDesiredVelocityChange[i] = bouncing_vel + velocities[i]; // Set the impulse bounds to not to be negative so that the impulse only // exerted to push the joint toward the positive direction. @@ -250,7 +250,7 @@ void JointConstraint::update() ++mDim; continue; - } else if (0 < B2) { + } else if (0 <= B2) { // The current position is greater than the upper bound. // // LB UB pos @@ -271,7 +271,7 @@ void JointConstraint::update() assert(bouncing_vel <= 0); bouncing_vel = std::max(bouncing_vel, -mMaxErrorReductionVelocity); - mDesiredVelocityChange[i] = bouncing_vel - velocities[i]; + mDesiredVelocityChange[i] = bouncing_vel + velocities[i]; // Set the impulse bounds to not to be positive so that the impulse only // exerted to push the joint toward the negative direction. From 8911246650a8d9db879c802d22a5b970d8b86f2d Mon Sep 17 00:00:00 2001 From: Haidar Obeid <110574820+ufrhaidar@users.noreply.github.com> Date: Thu, 27 Jun 2024 14:11:38 +1000 Subject: [PATCH 2/3] undo previous change --- dart/constraint/JointConstraint.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/dart/constraint/JointConstraint.cpp b/dart/constraint/JointConstraint.cpp index b0fcf91a584cc..32ebec57fdfc1 100644 --- a/dart/constraint/JointConstraint.cpp +++ b/dart/constraint/JointConstraint.cpp @@ -213,7 +213,7 @@ void JointConstraint::update() const double B1 = A1 + mErrorAllowance; const double A2 = positions[i] - positionUpperLimits[i]; const double B2 = A2 - mErrorAllowance; - if (B1 <= 0) { + if (B1 < 0) { // The current position is lower than the lower bound. // // pos LB UB @@ -234,7 +234,7 @@ void JointConstraint::update() assert(bouncing_vel >= 0); bouncing_vel = std::min(bouncing_vel, mMaxErrorReductionVelocity); - mDesiredVelocityChange[i] = bouncing_vel + velocities[i]; + mDesiredVelocityChange[i] = bouncing_vel - velocities[i]; // Set the impulse bounds to not to be negative so that the impulse only // exerted to push the joint toward the positive direction. @@ -250,7 +250,7 @@ void JointConstraint::update() ++mDim; continue; - } else if (0 <= B2) { + } else if (0 < B2) { // The current position is greater than the upper bound. // // LB UB pos @@ -271,7 +271,7 @@ void JointConstraint::update() assert(bouncing_vel <= 0); bouncing_vel = std::max(bouncing_vel, -mMaxErrorReductionVelocity); - mDesiredVelocityChange[i] = bouncing_vel + velocities[i]; + mDesiredVelocityChange[i] = bouncing_vel - velocities[i]; // Set the impulse bounds to not to be positive so that the impulse only // exerted to push the joint toward the negative direction. From a18343a7f2baf361bc795cd9547743e14dc5ce82 Mon Sep 17 00:00:00 2001 From: Haidar Obeid <110574820+ufrhaidar@users.noreply.github.com> Date: Thu, 27 Jun 2024 14:15:11 +1000 Subject: [PATCH 3/3] ignore velocity limits for servo motor --- dart/constraint/JointConstraint.cpp | 69 +++++++++++++++-------------- 1 file changed, 36 insertions(+), 33 deletions(-) diff --git a/dart/constraint/JointConstraint.cpp b/dart/constraint/JointConstraint.cpp index 32ebec57fdfc1..c888b2d1fabea 100644 --- a/dart/constraint/JointConstraint.cpp +++ b/dart/constraint/JointConstraint.cpp @@ -289,42 +289,45 @@ void JointConstraint::update() continue; } - // Check lower velocity bound - const double vel_lb = std::max(velocityLowerLimits[i], vel_to_pos_lb); - const double vel_lb_error = velocities[i] - vel_lb; - if (vel_lb_error < 0.0) { - mDesiredVelocityChange[i] = -vel_lb_error; - mImpulseLowerBound[i] = 0.0; - mImpulseUpperBound[i] = static_cast(dInfinity); - - if (mActive[i]) { - ++(mLifeTime[i]); - } else { - mActive[i] = true; - mLifeTime[i] = 0; + // Ignore for Servo motor + if (mJoint->getActuatorType() != dynamics::Joint::SERVO) { + // Check lower velocity bound + const double vel_lb = std::max(velocityLowerLimits[i], vel_to_pos_lb); + const double vel_lb_error = velocities[i] - vel_lb; + if (vel_lb_error < 0.0) { + mDesiredVelocityChange[i] = -vel_lb_error; + mImpulseLowerBound[i] = 0.0; + mImpulseUpperBound[i] = static_cast(dInfinity); + + if (mActive[i]) { + ++(mLifeTime[i]); + } else { + mActive[i] = true; + mLifeTime[i] = 0; + } + + ++mDim; + continue; } - ++mDim; - continue; - } - - // Check upper velocity bound - const double vel_ub = std::min(velocityUpperLimits[i], vel_to_pos_ub); - const double vel_ub_error = velocities[i] - vel_ub; - if (vel_ub_error > 0.0) { - mDesiredVelocityChange[i] = -vel_ub_error; - mImpulseLowerBound[i] = -static_cast(dInfinity); - mImpulseUpperBound[i] = 0.0; - - if (mActive[i]) { - ++(mLifeTime[i]); - } else { - mActive[i] = true; - mLifeTime[i] = 0; + // Check upper velocity bound + const double vel_ub = std::min(velocityUpperLimits[i], vel_to_pos_ub); + const double vel_ub_error = velocities[i] - vel_ub; + if (vel_ub_error > 0.0) { + mDesiredVelocityChange[i] = -vel_ub_error; + mImpulseLowerBound[i] = -static_cast(dInfinity); + mImpulseUpperBound[i] = 0.0; + + if (mActive[i]) { + ++(mLifeTime[i]); + } else { + mActive[i] = true; + mLifeTime[i] = 0; + } + + ++mDim; + continue; } - - ++mDim; - continue; } }