diff --git a/CHANGELOG.md b/CHANGELOG.md index ebc75e3ed9dc5..4df1cfce3f976 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -15,6 +15,10 @@ * Fixed incorrect MultiSphereConvexHull rendering: [#1579](https://github.com/dartsim/dart/pull/1579) * Use GLVND over the legacy OpenGL libraries: [#1584](https://github.com/dartsim/dart/pull/1584) +* Build and testing + + * Add DART_ prefix to macros to avoid potential conflicts: [#1586](https://github.com/dartsim/dart/pull/1586) + ### [DART 6.10.1 (2021-04-19)](https://github.com/dartsim/dart/milestone/65?closed=1) * Dynamics diff --git a/dart/constraint/BallJointConstraint.cpp b/dart/constraint/BallJointConstraint.cpp index 41e1507ebf8e7..7197b45d1c19f 100644 --- a/dart/constraint/BallJointConstraint.cpp +++ b/dart/constraint/BallJointConstraint.cpp @@ -43,7 +43,7 @@ namespace constraint { //============================================================================== BallJointConstraint::BallJointConstraint( dynamics::BodyNode* _body, const Eigen::Vector3d& _jointPos) - : JointConstraint(_body), + : DynamicJointConstraint(_body), mOffset1(_body->getTransform().inverse() * _jointPos), mOffset2(_jointPos), mViolation(Eigen::Vector3d::Zero()), @@ -65,7 +65,7 @@ BallJointConstraint::BallJointConstraint( dynamics::BodyNode* _body1, dynamics::BodyNode* _body2, const Eigen::Vector3d& _jointPos) - : JointConstraint(_body1, _body2), + : DynamicJointConstraint(_body1, _body2), mOffset1(_body1->getTransform().inverse() * _jointPos), mOffset2(_body2->getTransform().inverse() * _jointPos), mViolation(Eigen::Vector3d::Zero()), diff --git a/dart/constraint/BallJointConstraint.hpp b/dart/constraint/BallJointConstraint.hpp index 0ab593b97fbd3..a14bfcf5f4dd1 100644 --- a/dart/constraint/BallJointConstraint.hpp +++ b/dart/constraint/BallJointConstraint.hpp @@ -35,7 +35,7 @@ #include -#include "dart/constraint/JointConstraint.hpp" +#include "dart/constraint/DynamicJointConstraint.hpp" #include "dart/math/MathTypes.hpp" namespace dart { @@ -43,7 +43,7 @@ namespace constraint { /// BallJointConstraint represents ball joint constraint between a body and the /// world or between two bodies -class BallJointConstraint : public JointConstraint +class BallJointConstraint : public DynamicJointConstraint { public: /// Constructor that takes one body and the joint position in the world frame diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index f3f1dd6ac2a61..49c25a6c2ca89 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -43,11 +43,10 @@ #include "dart/common/Console.hpp" #include "dart/constraint/ConstrainedGroup.hpp" #include "dart/constraint/ContactConstraint.hpp" +#include "dart/constraint/JointConstraint.hpp" #include "dart/constraint/JointCoulombFrictionConstraint.hpp" -#include "dart/constraint/JointLimitConstraint.hpp" #include "dart/constraint/LCPSolver.hpp" #include "dart/constraint/MimicMotorConstraint.hpp" -#include "dart/constraint/ServoMotorConstraint.hpp" #include "dart/constraint/SoftContactConstraint.hpp" #include "dart/dynamics/BodyNode.hpp" #include "dart/dynamics/Joint.hpp" @@ -588,8 +587,7 @@ void ConstraintSolver::updateConstraints() // Update automatic constraints: joint constraints //---------------------------------------------------------------------------- // Destroy previous joint constraints - mJointLimitConstraints.clear(); - mServoMotorConstraints.clear(); + mJointConstraints.clear(); mMimicMotorConstraints.clear(); mJointCoulombFrictionConstraints.clear(); @@ -615,16 +613,10 @@ void ConstraintSolver::updateConstraints() } } - if (joint->areLimitsEnforced()) + if (joint->areLimitsEnforced() + || joint->getActuatorType() == dynamics::Joint::SERVO) { - mJointLimitConstraints.push_back( - std::make_shared(joint)); - } - - if (joint->getActuatorType() == dynamics::Joint::SERVO) - { - mServoMotorConstraints.push_back( - std::make_shared(joint)); + mJointConstraints.push_back(std::make_shared(joint)); } if (joint->getActuatorType() == dynamics::Joint::MIMIC @@ -640,7 +632,7 @@ void ConstraintSolver::updateConstraints() } // Add active joint limit - for (auto& jointLimitConstraint : mJointLimitConstraints) + for (auto& jointLimitConstraint : mJointConstraints) { jointLimitConstraint->update(); @@ -648,14 +640,6 @@ void ConstraintSolver::updateConstraints() mActiveConstraints.push_back(jointLimitConstraint); } - for (auto& servoMotorConstraint : mServoMotorConstraints) - { - servoMotorConstraint->update(); - - if (servoMotorConstraint->isActive()) - mActiveConstraints.push_back(servoMotorConstraint); - } - for (auto& mimicMotorConstraint : mMimicMotorConstraints) { mimicMotorConstraint->update(); diff --git a/dart/constraint/ConstraintSolver.hpp b/dart/constraint/ConstraintSolver.hpp index 1ad6198350d8b..d8775ac8598cf 100644 --- a/dart/constraint/ConstraintSolver.hpp +++ b/dart/constraint/ConstraintSolver.hpp @@ -239,10 +239,7 @@ class ConstraintSolver std::vector mSoftContactConstraints; /// Joint limit constraints those are automatically created - std::vector mJointLimitConstraints; - - /// Servo motor constraints those are automatically created - std::vector mServoMotorConstraints; + std::vector mJointConstraints; /// Mimic motor constraints those are automatically created std::vector mMimicMotorConstraints; diff --git a/dart/constraint/DynamicJointConstraint.cpp b/dart/constraint/DynamicJointConstraint.cpp new file mode 100644 index 0000000000000..abd6aed195fa0 --- /dev/null +++ b/dart/constraint/DynamicJointConstraint.cpp @@ -0,0 +1,176 @@ +/* + * Copyright (c) 2011-2021, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/constraint/DynamicJointConstraint.hpp" + +#include + +#include "dart/common/Console.hpp" + +#define DART_ERROR_ALLOWANCE 0.0 +#define DART_ERP 0.01 +#define DART_MAX_ERV 1e+1 +#define DART_CFM 1e-9 + +namespace dart { +namespace constraint { + +double DynamicJointConstraint::mErrorAllowance = DART_ERROR_ALLOWANCE; +double DynamicJointConstraint::mErrorReductionParameter = DART_ERP; +double DynamicJointConstraint::mMaxErrorReductionVelocity = DART_MAX_ERV; +double DynamicJointConstraint::mConstraintForceMixing = DART_CFM; + +//============================================================================== +DynamicJointConstraint::DynamicJointConstraint(dynamics::BodyNode* body) + : ConstraintBase(), mBodyNode1(body), mBodyNode2(nullptr) +{ + assert(body); +} + +//============================================================================== +DynamicJointConstraint::DynamicJointConstraint( + dynamics::BodyNode* body1, dynamics::BodyNode* body2) + : ConstraintBase(), mBodyNode1(body1), mBodyNode2(body2) +{ + assert(body1); + assert(body2); +} + +//============================================================================== +DynamicJointConstraint::~DynamicJointConstraint() +{ + // Do nothing +} + +//============================================================================== +void DynamicJointConstraint::setErrorAllowance(double allowance) +{ + // Clamp error reduction parameter if it is out of the range + if (allowance < 0.0) + { + dtwarn << "Error reduction parameter[" << allowance + << "] is lower than 0.0. " + << "It is set to 0.0." << std::endl; + mErrorAllowance = 0.0; + } + + mErrorAllowance = allowance; +} + +//============================================================================== +double DynamicJointConstraint::getErrorAllowance() +{ + return mErrorAllowance; +} + +//============================================================================== +void DynamicJointConstraint::setErrorReductionParameter(double erp) +{ + // Clamp error reduction parameter if it is out of the range [0, 1] + if (erp < 0.0) + { + dtwarn << "Error reduction parameter [" << erp << "] is lower than 0.0. " + << "It is set to 0.0." << std::endl; + mErrorReductionParameter = 0.0; + } + if (erp > 1.0) + { + dtwarn << "Error reduction parameter [" << erp << "] is greater than 1.0. " + << "It is set to 1.0." << std::endl; + mErrorReductionParameter = 1.0; + } + + mErrorReductionParameter = erp; +} + +//============================================================================== +double DynamicJointConstraint::getErrorReductionParameter() +{ + return mErrorReductionParameter; +} + +//============================================================================== +void DynamicJointConstraint::setMaxErrorReductionVelocity(double erv) +{ + // Clamp maximum error reduction velocity if it is out of the range + if (erv < 0.0) + { + dtwarn << "Maximum error reduction velocity [" << erv + << "] is lower than 0.0. " + << "It is set to 0.0." << std::endl; + mMaxErrorReductionVelocity = 0.0; + } + + mMaxErrorReductionVelocity = erv; +} + +//============================================================================== +double DynamicJointConstraint::getMaxErrorReductionVelocity() +{ + return mMaxErrorReductionVelocity; +} + +//============================================================================== +void DynamicJointConstraint::setConstraintForceMixing(double cfm) +{ + // Clamp constraint force mixing parameter if it is out of the range + if (cfm < 1e-9) + { + dtwarn << "Constraint force mixing parameter [" << cfm + << "] is lower than 1e-9. " + << "It is set to 1e-9." << std::endl; + mConstraintForceMixing = 1e-9; + } + + mConstraintForceMixing = cfm; +} + +//============================================================================== +double DynamicJointConstraint::getConstraintForceMixing() +{ + return mConstraintForceMixing; +} + +//============================================================================== +dynamics::BodyNode* DynamicJointConstraint::getBodyNode1() const +{ + return mBodyNode1; +} + +//============================================================================== +dynamics::BodyNode* DynamicJointConstraint::getBodyNode2() const +{ + return mBodyNode2; +} + +} // namespace constraint +} // namespace dart diff --git a/dart/constraint/DynamicJointConstraint.hpp b/dart/constraint/DynamicJointConstraint.hpp new file mode 100644 index 0000000000000..182f3d1e676f7 --- /dev/null +++ b/dart/constraint/DynamicJointConstraint.hpp @@ -0,0 +1,120 @@ +/* + * Copyright (c) 2011-2021, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_CONSTRAINT_DYNAMICJOINTCONSTRAINT_HPP_ +#define DART_CONSTRAINT_DYNAMICJOINTCONSTRAINT_HPP_ + +#include "dart/constraint/ConstraintBase.hpp" + +namespace dart { + +namespace dynamics { +class BodyNode; +} // namespace dynamics + +namespace constraint { + +/// Base class for joint constraints that are being created or destructed during +/// simulation. +class DynamicJointConstraint : public ConstraintBase +{ +public: + /// Contructor + explicit DynamicJointConstraint(dynamics::BodyNode* body); + + /// Contructor + DynamicJointConstraint(dynamics::BodyNode* body1, dynamics::BodyNode* body2); + + /// Destructor + ~DynamicJointConstraint() override; + + //---------------------------------------------------------------------------- + // Property settings + //---------------------------------------------------------------------------- + + /// Set global error reduction parameter + static void setErrorAllowance(double allowance); + + /// Get global error reduction parameter + static double getErrorAllowance(); + + /// Set global error reduction parameter + static void setErrorReductionParameter(double erp); + + /// Get global error reduction parameter + static double getErrorReductionParameter(); + + /// Set global error reduction parameter + static void setMaxErrorReductionVelocity(double erv); + + /// Get global error reduction parameter + static double getMaxErrorReductionVelocity(); + + /// Set global constraint force mixing parameter + static void setConstraintForceMixing(double cfm); + + /// Get global constraint force mixing parameter + static double getConstraintForceMixing(); + + /// Get the first BodyNode that this constraint is associated with + dynamics::BodyNode* getBodyNode1() const; + + /// Get the second BodyNode that this constraint is associated with + dynamics::BodyNode* getBodyNode2() const; + +protected: + /// First body node + dynamics::BodyNode* mBodyNode1; + + /// Second body node + dynamics::BodyNode* mBodyNode2; + + /// Global constraint error allowance + static double mErrorAllowance; + + /// Global constraint error redection parameter in the range of [0, 1]. The + /// default is 0.01. + static double mErrorReductionParameter; + + /// Maximum error reduction velocity + static double mMaxErrorReductionVelocity; + + /// Global constraint force mixing parameter in the range of [1e-9, 1]. The + /// default is 1e-5 + /// \sa http://www.ode.org/ode-latest-userguide.html#sec_3_8_0 + static double mConstraintForceMixing; +}; + +} // namespace constraint +} // namespace dart + +#endif // DART_CONSTRAINT_DYNAMICJOINTCONSTRAINT_HPP_ diff --git a/dart/constraint/JointConstraint.cpp b/dart/constraint/JointConstraint.cpp index b5eca004b4caf..f8e8260407c45 100644 --- a/dart/constraint/JointConstraint.cpp +++ b/dart/constraint/JointConstraint.cpp @@ -32,10 +32,13 @@ #include "dart/constraint/JointConstraint.hpp" -#include -#include +#include #include "dart/common/Console.hpp" +#include "dart/dynamics/BodyNode.hpp" +#include "dart/dynamics/Joint.hpp" +#include "dart/dynamics/Skeleton.hpp" +#include "dart/external/odelcpsolver/lcp.h" #define DART_ERROR_ALLOWANCE 0.0 #define DART_ERP 0.01 @@ -51,39 +54,44 @@ double JointConstraint::mMaxErrorReductionVelocity = DART_MAX_ERV; double JointConstraint::mConstraintForceMixing = DART_CFM; //============================================================================== -JointConstraint::JointConstraint(dynamics::BodyNode* _body) - : ConstraintBase(), mBodyNode1(_body), mBodyNode2(nullptr) +JointConstraint::JointConstraint(dynamics::Joint* joint) + : ConstraintBase(), + mJoint(joint), + mBodyNode(joint->getChildBodyNode()), + mAppliedImpulseIndex(0) { - assert(_body); + assert(joint); + assert(mBodyNode); + mLifeTime.setZero(); + mActive.setConstant(false); } //============================================================================== -JointConstraint::JointConstraint( - dynamics::BodyNode* _body1, dynamics::BodyNode* _body2) - : ConstraintBase(), mBodyNode1(_body1), mBodyNode2(_body2) +const std::string& JointConstraint::getType() const { - assert(_body1); - assert(_body2); + return getStaticType(); } //============================================================================== -JointConstraint::~JointConstraint() +const std::string& JointConstraint::getStaticType() { + static const std::string name = "JointConstraint"; + return name; } //============================================================================== -void JointConstraint::setErrorAllowance(double _allowance) +void JointConstraint::setErrorAllowance(double allowance) { // Clamp error reduction parameter if it is out of the range - if (_allowance < 0.0) + if (allowance < 0.0) { - dtwarn << "Error reduction parameter[" << _allowance + dtwarn << "Error reduction parameter [" << allowance << "] is lower than 0.0. " << "It is set to 0.0." << std::endl; mErrorAllowance = 0.0; } - mErrorAllowance = _allowance; + mErrorAllowance = allowance; } //============================================================================== @@ -93,23 +101,23 @@ double JointConstraint::getErrorAllowance() } //============================================================================== -void JointConstraint::setErrorReductionParameter(double _erp) +void JointConstraint::setErrorReductionParameter(double erp) { // Clamp error reduction parameter if it is out of the range [0, 1] - if (_erp < 0.0) + if (erp < 0.0) { - dtwarn << "Error reduction parameter[" << _erp << "] is lower than 0.0. " + dtwarn << "Error reduction parameter [" << erp << "] is lower than 0.0. " << "It is set to 0.0." << std::endl; mErrorReductionParameter = 0.0; } - if (_erp > 1.0) + if (erp > 1.0) { - dtwarn << "Error reduction parameter[" << _erp << "] is greater than 1.0. " + dtwarn << "Error reduction parameter [" << erp << "] is greater than 1.0. " << "It is set to 1.0." << std::endl; mErrorReductionParameter = 1.0; } - mErrorReductionParameter = _erp; + mErrorReductionParameter = erp; } //============================================================================== @@ -119,18 +127,18 @@ double JointConstraint::getErrorReductionParameter() } //============================================================================== -void JointConstraint::setMaxErrorReductionVelocity(double _erv) +void JointConstraint::setMaxErrorReductionVelocity(double erv) { // Clamp maximum error reduction velocity if it is out of the range - if (_erv < 0.0) + if (erv < 0.0) { - dtwarn << "Maximum error reduction velocity[" << _erv + dtwarn << "Maximum error reduction velocity [" << erv << "] is lower than 0.0. " << "It is set to 0.0." << std::endl; mMaxErrorReductionVelocity = 0.0; } - mMaxErrorReductionVelocity = _erv; + mMaxErrorReductionVelocity = erv; } //============================================================================== @@ -140,18 +148,18 @@ double JointConstraint::getMaxErrorReductionVelocity() } //============================================================================== -void JointConstraint::setConstraintForceMixing(double _cfm) +void JointConstraint::setConstraintForceMixing(double cfm) { // Clamp constraint force mixing parameter if it is out of the range - if (_cfm < 1e-9) + if (cfm < 1e-9) { - dtwarn << "Constraint force mixing parameter[" << _cfm + dtwarn << "Constraint force mixing parameter [" << cfm << "] is lower than 1e-9. " << "It is set to 1e-9." << std::endl; mConstraintForceMixing = 1e-9; } - mConstraintForceMixing = _cfm; + mConstraintForceMixing = cfm; } //============================================================================== @@ -161,15 +169,362 @@ double JointConstraint::getConstraintForceMixing() } //============================================================================== -dynamics::BodyNode* JointConstraint::getBodyNode1() const +void JointConstraint::update() { - return mBodyNode1; + // Reset dimension + mDim = 0; + + const int dof = static_cast(mJoint->getNumDofs()); + + const Eigen::VectorXd positions = mJoint->getPositions(); + const Eigen::VectorXd velocities = mJoint->getVelocities(); + + const Eigen::VectorXd positionLowerLimits = mJoint->getPositionLowerLimits(); + const Eigen::VectorXd positionUpperLimits = mJoint->getPositionUpperLimits(); + + const Eigen::VectorXd velocityLowerLimits = mJoint->getVelocityLowerLimits(); + const Eigen::VectorXd velocityUpperLimits = mJoint->getVelocityUpperLimits(); + + const double timeStep = mJoint->getSkeleton()->getTimeStep(); + // TODO: There are multiple ways to get time step (or its inverse). + // - ContactConstraint get it from the constructor parameter + // - Skeleton has it itself. + // - ConstraintBase::getInformation() passes ConstraintInfo structure + // that contains reciprocal time step. + // We might need to pick one way and remove the others to get rid of + // redundancy. + + // Constraint priority: + // + // 1. Position limits + // 2. Velocity limits + // 3. Servo motor + + mActive.setConstant(false); + + for (int i = 0; i < dof; ++i) + { + assert(positionLowerLimits[i] <= positionUpperLimits[i]); + assert(velocityLowerLimits[i] <= velocityUpperLimits[i]); + + // Velocity limits due to position limits + const double vel_to_pos_lb + = (positionLowerLimits[i] - positions[i]) / timeStep; + const double vel_to_pos_ub + = (positionUpperLimits[i] - positions[i]) / timeStep; + + // Joint position and velocity constraint check + if (mJoint->areLimitsEnforced()) + { + const double A1 = positions[i] - positionLowerLimits[i]; + const double B1 = A1 + mErrorAllowance; + const double A2 = positions[i] - positionUpperLimits[i]; + const double B2 = A2 - mErrorAllowance; + if (B1 < 0) + { + // The current position is lower than the lower bound. + // + // pos LB UB + // | | | + // v v v + // ----+---------+----+-------------------------------+---- + // |--->| : error_allowance + // |<-------------| : A1 + // |<--------| : B1 = A1 + error_allowance + // |<----~~~---| : C1 = ERP * A1 + + // Set the desired velocity change to the negated current velocity plus + // the bouncing velocity so that the joint stops and bounces to correct + // the position error. + + const double C1 = mErrorAllowance * A1; + double bouncing_vel = -std::max(B1, C1) / timeStep; + assert(bouncing_vel >= 0); + bouncing_vel = std::min(bouncing_vel, mMaxErrorReductionVelocity); + + 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. + mImpulseLowerBound[i] = 0.0; + mImpulseUpperBound[i] = static_cast(dInfinity); + + if (mActive[i]) + { + ++(mLifeTime[i]); + } + else + { + mActive[i] = true; + mLifeTime[i] = 0; + } + + ++mDim; + continue; + } + else if (0 < B2) + { + // The current position is greater than the upper bound. + // + // LB UB pos + // | | | + // v v v + // ----+-------------------------------+----+---------+---- + // error_allowance : |--->| + // A2 : |------------->| + // B2 = A2 - error_allowance : |-------->| + // C2 = ERP * A2 : |----~~~--->| + + // Set the desired velocity change to the negated current velocity plus + // the bouncing velocity so that the joint stops and bounces to correct + // the position error. + + const double C2 = mErrorAllowance * A2; + double bouncing_vel = -std::min(B2, C2) / timeStep; + assert(bouncing_vel <= 0); + bouncing_vel = std::max(bouncing_vel, -mMaxErrorReductionVelocity); + + 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. + mImpulseLowerBound[i] = -static_cast(dInfinity); + mImpulseUpperBound[i] = 0.0; + + if (mActive[i]) + { + ++(mLifeTime[i]); + } + else + { + mActive[i] = true; + mLifeTime[i] = 0; + } + + ++mDim; + 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; + } + + ++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; + } + + ++mDim; + continue; + } + } + + // Servo motor constraint check + if (mJoint->getActuatorType() == dynamics::Joint::SERVO) + { + // The desired velocity shouldn't be out of the velocity limits + double desired_velocity = math::clip( + mJoint->getCommand(static_cast(i)), + velocityLowerLimits[i], + velocityUpperLimits[i]); + + // The next position shouldn't be out of the position limits + desired_velocity + = math::clip(desired_velocity, vel_to_pos_lb, vel_to_pos_ub); + + mDesiredVelocityChange[i] = desired_velocity - velocities[i]; + + if (mDesiredVelocityChange[i] != 0.0) + { + // Note that we are computing impulse but not force + mImpulseUpperBound[i] + = mJoint->getForceUpperLimit(static_cast(i)) + * timeStep; + mImpulseLowerBound[i] + = mJoint->getForceLowerLimit(static_cast(i)) + * timeStep; + + if (mActive[i]) + { + ++(mLifeTime[i]); + } + else + { + mActive[i] = true; + mLifeTime[i] = 0; + } + + ++mDim; + } + } + } +} + +//============================================================================== +void JointConstraint::getInformation(ConstraintInfo* lcp) +{ + std::size_t index = 0; + const int dof = static_cast(mJoint->getNumDofs()); + for (int i = 0; i < dof; ++i) + { + assert(lcp->w[index] == 0.0); + + lcp->b[index] = mDesiredVelocityChange[i]; + + lcp->lo[index] = mImpulseLowerBound[i]; + lcp->hi[index] = mImpulseUpperBound[i]; + + assert(lcp->findex[index] == -1); + + if (mLifeTime[i]) + lcp->x[index] = mOldX[i]; + else + lcp->x[index] = 0.0; + + index++; + } +} + +//============================================================================== +void JointConstraint::applyUnitImpulse(std::size_t index) +{ + assert(index < mDim && "Invalid Index."); + + std::size_t localIndex = 0; + const dynamics::SkeletonPtr& skeleton = mJoint->getSkeleton(); + + std::size_t dof = mJoint->getNumDofs(); + for (std::size_t i = 0; i < dof; ++i) + { + if (!mActive[static_cast(i)]) + { + continue; + } + + if (localIndex == index) + { + skeleton->clearConstraintImpulses(); + mJoint->setConstraintImpulse(i, 1.0); + skeleton->updateBiasImpulse(mBodyNode); + skeleton->updateVelocityChange(); + mJoint->setConstraintImpulse(i, 0.0); + } + + ++localIndex; + } + + mAppliedImpulseIndex = index; +} + +//============================================================================== +void JointConstraint::getVelocityChange(double* delVel, bool withCfm) +{ + assert(delVel != nullptr && "Null pointer is not allowed."); + + std::size_t localIndex = 0; + std::size_t dof = mJoint->getNumDofs(); + for (std::size_t i = 0; i < dof; ++i) + { + if (!mActive[static_cast(i)]) + { + continue; + } + + if (mJoint->getSkeleton()->isImpulseApplied()) + delVel[localIndex] = mJoint->getVelocityChange(i); + else + delVel[localIndex] = 0.0; + + ++localIndex; + } + + // Add small values to diagnal to keep it away from singular, similar to cfm + // varaible in ODE + if (withCfm) + { + delVel[mAppliedImpulseIndex] + += delVel[mAppliedImpulseIndex] * mConstraintForceMixing; + } + + assert(localIndex == mDim); +} + +//============================================================================== +void JointConstraint::excite() +{ + mJoint->getSkeleton()->setImpulseApplied(true); +} + +//============================================================================== +void JointConstraint::unexcite() +{ + mJoint->getSkeleton()->setImpulseApplied(false); +} + +//============================================================================== +void JointConstraint::applyImpulse(double* lambda) +{ + std::size_t localIndex = 0; + std::size_t dof = mJoint->getNumDofs(); + for (std::size_t i = 0; i < dof; ++i) + { + if (!mActive[static_cast(i)]) + { + continue; + } + + mJoint->setConstraintImpulse( + i, mJoint->getConstraintImpulse(i) + lambda[localIndex]); + + mOldX[static_cast(i)] = lambda[localIndex]; + + ++localIndex; + } +} + +//============================================================================== +dynamics::SkeletonPtr JointConstraint::getRootSkeleton() const +{ + return mJoint->getSkeleton()->mUnionRootSkeleton.lock(); } //============================================================================== -dynamics::BodyNode* JointConstraint::getBodyNode2() const +bool JointConstraint::isActive() const { - return mBodyNode2; + return mActive.array().any(); } } // namespace constraint diff --git a/dart/constraint/JointConstraint.hpp b/dart/constraint/JointConstraint.hpp index da7ab996e4848..fe9fc89ae07d0 100644 --- a/dart/constraint/JointConstraint.hpp +++ b/dart/constraint/JointConstraint.hpp @@ -33,69 +33,132 @@ #ifndef DART_CONSTRAINT_JOINTCONSTRAINT_HPP_ #define DART_CONSTRAINT_JOINTCONSTRAINT_HPP_ +#include + #include "dart/constraint/ConstraintBase.hpp" namespace dart { namespace dynamics { class BodyNode; +class Joint; } // namespace dynamics namespace constraint { -/// class JointConstraint +// TODO(JS): Merge JointCoulombFrictionConstraint into this class + +/// JointConstraint handles multiple constraints that are defined in the joint +/// space, such as joint position/velocity limits and servo motor. class JointConstraint : public ConstraintBase { public: - /// Contructor - explicit JointConstraint(dynamics::BodyNode* _body); - - /// Contructor - JointConstraint(dynamics::BodyNode* _body1, dynamics::BodyNode* _body2); + /// Constructor + explicit JointConstraint(dynamics::Joint* joint); /// Destructor - virtual ~JointConstraint(); + ~JointConstraint() override = default; + + // Documentation inherited + const std::string& getType() const override; + + /// Returns constraint type for this class. + static const std::string& getStaticType(); //---------------------------------------------------------------------------- // Property settings //---------------------------------------------------------------------------- /// Set global error reduction parameter - static void setErrorAllowance(double _allowance); + static void setErrorAllowance(double allowance); /// Get global error reduction parameter static double getErrorAllowance(); /// Set global error reduction parameter - static void setErrorReductionParameter(double _erp); + static void setErrorReductionParameter(double erp); /// Get global error reduction parameter static double getErrorReductionParameter(); /// Set global error reduction parameter - static void setMaxErrorReductionVelocity(double _erv); + static void setMaxErrorReductionVelocity(double erv); /// Get global error reduction parameter static double getMaxErrorReductionVelocity(); /// Set global constraint force mixing parameter - static void setConstraintForceMixing(double _cfm); + static void setConstraintForceMixing(double cfm); /// Get global constraint force mixing parameter static double getConstraintForceMixing(); - /// Get the first BodyNode that this constraint is associated with - dynamics::BodyNode* getBodyNode1() const; + //---------------------------------------------------------------------------- + // Friendship + //---------------------------------------------------------------------------- - /// Get the second BodyNode that this constraint is associated with - dynamics::BodyNode* getBodyNode2() const; + friend class ConstraintSolver; + friend class ConstrainedGroup; protected: - /// First body node - dynamics::BodyNode* mBodyNode1; + //---------------------------------------------------------------------------- + // Constraint virtual functions + //---------------------------------------------------------------------------- + + // Documentation inherited + void update() override; + + // Documentation inherited + void getInformation(ConstraintInfo* lcp) override; + + // Documentation inherited + void applyUnitImpulse(std::size_t index) override; + + // Documentation inherited + void getVelocityChange(double* delVel, bool withCfm) override; + + // Documentation inherited + void excite() override; + + // Documentation inherited + void unexcite() override; + + // Documentation inherited + void applyImpulse(double* lambda) override; + + // Documentation inherited + dynamics::SkeletonPtr getRootSkeleton() const override; + + // Documentation inherited + bool isActive() const override; + +private: + /// The Joint that this constraint is associated with. + dynamics::Joint* mJoint; + + /// The child BodyNode of the Joint that this constraint is associated with. + dynamics::BodyNode* mBodyNode; + + /// Index of applied impulse + std::size_t mAppliedImpulseIndex; + + /// Life time of constraint of each DOF. + Eigen::Matrix mLifeTime; + + /// Whether any of the joint contraint is active of each joint. + Eigen::Matrix mActive; + + /// The desired delta velocity to satisfy the joint limit constraint. + Eigen::Matrix mDesiredVelocityChange; + + /// Constraint impulse of the previous step. + Eigen::Matrix mOldX; + + /// Upper limit of the constraint impulse. + Eigen::Matrix mImpulseUpperBound; - /// Second body node - dynamics::BodyNode* mBodyNode2; + /// Lower limit of the constraint impulse. + Eigen::Matrix mImpulseLowerBound; /// Global constraint error allowance static double mErrorAllowance; @@ -116,4 +179,4 @@ class JointConstraint : public ConstraintBase } // namespace constraint } // namespace dart -#endif // DART_CONSTRAINT_CONSTRAINT_HPP_ +#endif // DART_CONSTRAINT_JOINTCONSTRAINT_HPP_ diff --git a/dart/constraint/SmartPointer.hpp b/dart/constraint/SmartPointer.hpp index af389e3ee2cc5..c4cf86aae542b 100644 --- a/dart/constraint/SmartPointer.hpp +++ b/dart/constraint/SmartPointer.hpp @@ -46,8 +46,7 @@ DART_COMMON_DECLARE_SHARED_WEAK(ConstraintBase) DART_COMMON_DECLARE_SHARED_WEAK(ClosedLoopConstraint) DART_COMMON_DECLARE_SHARED_WEAK(ContactConstraint) DART_COMMON_DECLARE_SHARED_WEAK(SoftContactConstraint) -DART_COMMON_DECLARE_SHARED_WEAK(JointLimitConstraint) -DART_COMMON_DECLARE_SHARED_WEAK(ServoMotorConstraint) +DART_COMMON_DECLARE_SHARED_WEAK(JointConstraint) DART_COMMON_DECLARE_SHARED_WEAK(MimicMotorConstraint) DART_COMMON_DECLARE_SHARED_WEAK(JointCoulombFrictionConstraint) @@ -57,7 +56,7 @@ DART_COMMON_DECLARE_SHARED_WEAK(PgsBoxedLcpSolver) DART_COMMON_DECLARE_SHARED_WEAK(PsorBoxedLcpSolver) DART_COMMON_DECLARE_SHARED_WEAK(JacobiBoxedLcpSolver) -DART_COMMON_DECLARE_SHARED_WEAK(JointConstraint) +DART_COMMON_DECLARE_SHARED_WEAK(DynamicJointConstraint) DART_COMMON_DECLARE_SHARED_WEAK(BallJointConstraint) DART_COMMON_DECLARE_SHARED_WEAK(WeldJointConstraint) diff --git a/dart/constraint/WeldJointConstraint.cpp b/dart/constraint/WeldJointConstraint.cpp index 7fb1474e375bb..8d3f3a6ef120d 100644 --- a/dart/constraint/WeldJointConstraint.cpp +++ b/dart/constraint/WeldJointConstraint.cpp @@ -42,7 +42,7 @@ namespace constraint { //============================================================================== WeldJointConstraint::WeldJointConstraint(dynamics::BodyNode* _body) - : JointConstraint(_body), + : DynamicJointConstraint(_body), mRelativeTransform(_body->getTransform()), mViolation(Eigen::Vector6d::Zero()), mJacobian1(Eigen::Matrix6d::Identity()), @@ -61,7 +61,7 @@ WeldJointConstraint::WeldJointConstraint(dynamics::BodyNode* _body) //============================================================================== WeldJointConstraint::WeldJointConstraint( dynamics::BodyNode* _body1, dynamics::BodyNode* _body2) - : JointConstraint(_body1, _body2), + : DynamicJointConstraint(_body1, _body2), mRelativeTransform( _body2->getTransform().inverse() * _body1->getTransform()), mViolation(Eigen::Vector6d::Zero()), diff --git a/dart/constraint/WeldJointConstraint.hpp b/dart/constraint/WeldJointConstraint.hpp index 762615f7126f2..21b90f5f6c65c 100644 --- a/dart/constraint/WeldJointConstraint.hpp +++ b/dart/constraint/WeldJointConstraint.hpp @@ -35,7 +35,7 @@ #include -#include "dart/constraint/JointConstraint.hpp" +#include "dart/constraint/DynamicJointConstraint.hpp" #include "dart/math/MathTypes.hpp" namespace dart { @@ -43,7 +43,7 @@ namespace constraint { /// WeldJointConstraint represents weld joint constraint between a body and the /// world or between two bodies -class WeldJointConstraint : public JointConstraint +class WeldJointConstraint : public DynamicJointConstraint { public: /// Constructor that takes one body diff --git a/dart/dynamics/PointCloudShape.cpp b/dart/dynamics/PointCloudShape.cpp index 82c4c1a91989a..c3c79554cc409 100644 --- a/dart/dynamics/PointCloudShape.cpp +++ b/dart/dynamics/PointCloudShape.cpp @@ -111,7 +111,7 @@ void PointCloudShape::setPoint(const std::vector& points) #if HAVE_OCTOMAP //============================================================================== -void PointCloudShape::setPoints(octomap::Pointcloud& pointCloud) +void PointCloudShape::setPoints(const ::octomap::Pointcloud& pointCloud) { mPoints.resize(pointCloud.size()); for (auto i = 0u; i < mPoints.size(); ++i) @@ -120,7 +120,7 @@ void PointCloudShape::setPoints(octomap::Pointcloud& pointCloud) } //============================================================================== -void PointCloudShape::addPoints(octomap::Pointcloud& pointCloud) +void PointCloudShape::addPoints(const ::octomap::Pointcloud& pointCloud) { mPoints.reserve(mPoints.size() + pointCloud.size()); for (const auto& point : pointCloud) diff --git a/dart/dynamics/PointCloudShape.hpp b/dart/dynamics/PointCloudShape.hpp index d33c46d18af1c..90b7e5236f424 100644 --- a/dart/dynamics/PointCloudShape.hpp +++ b/dart/dynamics/PointCloudShape.hpp @@ -97,10 +97,10 @@ class PointCloudShape : public Shape #if HAVE_OCTOMAP /// Replaces points with \c pointCloud. - void setPoints(::octomap::Pointcloud& pointCloud); + void setPoints(const ::octomap::Pointcloud& pointCloud); /// Adds points from Octomap PointCloud. - void addPoints(::octomap::Pointcloud& pointCloud); + void addPoints(const ::octomap::Pointcloud& pointCloud); #endif /// Returns the list of points. diff --git a/data/sdf/test/test_issue1583.model b/data/sdf/test/test_issue1583.model new file mode 100644 index 0000000000000..cf656d9ed87e0 --- /dev/null +++ b/data/sdf/test/test_issue1583.model @@ -0,0 +1,45 @@ + + + + 10 10 2 0 0 0 + + + 100 + + + 0 0 0 -1.57 0 0 + + + 0.1 + 0.2 + + + + + + 0 0 -1 0 0 0 + + 1 + + + + + 0.1 0.1 0.1 + + + + + + world + base + + + 0 0 1 0 0 0 + base + bar + + 0 1 0 + + + + diff --git a/python/dartpy/constraint/JointLimitConstraint.cpp b/python/dartpy/constraint/DynamicJointConstraint.cpp similarity index 51% rename from python/dartpy/constraint/JointLimitConstraint.cpp rename to python/dartpy/constraint/DynamicJointConstraint.cpp index 197393851ca91..848462d3ea056 100644 --- a/python/dartpy/constraint/JointLimitConstraint.cpp +++ b/python/dartpy/constraint/DynamicJointConstraint.cpp @@ -31,6 +31,7 @@ */ #include +#include #include namespace py = pybind11; @@ -38,63 +39,105 @@ namespace py = pybind11; namespace dart { namespace python { -void JointLimitConstraint(py::module& m) +void DynamicJointConstraint(py::module& m) { ::py::class_< - dart::constraint::JointLimitConstraint, + dart::constraint::DynamicJointConstraint, dart::constraint::ConstraintBase, - std::shared_ptr>( - m, "JointLimitConstraint") - .def(::py::init(), ::py::arg("joint")) + std::shared_ptr>( + m, "DynamicJointConstraint") .def_static( "setErrorAllowance", - +[](double _allowance) { - dart::constraint::JointLimitConstraint::setErrorAllowance( - _allowance); + +[](double allowance) { + dart::constraint::DynamicJointConstraint::setErrorAllowance( + allowance); }, ::py::arg("allowance")) .def_static( "getErrorAllowance", +[]() -> double { - return dart::constraint::JointLimitConstraint::getErrorAllowance(); + return dart::constraint::DynamicJointConstraint:: + getErrorAllowance(); }) .def_static( "setErrorReductionParameter", - +[](double _erp) { - dart::constraint::JointLimitConstraint::setErrorReductionParameter( - _erp); + +[](double erp) { + dart::constraint::DynamicJointConstraint:: + setErrorReductionParameter(erp); }, ::py::arg("erp")) .def_static( "getErrorReductionParameter", +[]() -> double { - return dart::constraint::JointLimitConstraint:: + return dart::constraint::DynamicJointConstraint:: getErrorReductionParameter(); }) .def_static( "setMaxErrorReductionVelocity", - +[](double _erv) { - dart::constraint::JointLimitConstraint:: - setMaxErrorReductionVelocity(_erv); + +[](double erv) { + dart::constraint::DynamicJointConstraint:: + setMaxErrorReductionVelocity(erv); }, ::py::arg("erv")) .def_static( "getMaxErrorReductionVelocity", +[]() -> double { - return dart::constraint::JointLimitConstraint:: + return dart::constraint::DynamicJointConstraint:: getMaxErrorReductionVelocity(); }) .def_static( "setConstraintForceMixing", - +[](double _cfm) { - dart::constraint::JointLimitConstraint::setConstraintForceMixing( - _cfm); + +[](double cfm) { + dart::constraint::DynamicJointConstraint::setConstraintForceMixing( + cfm); }, ::py::arg("cfm")) .def_static("getConstraintForceMixing", +[]() -> double { - return dart::constraint::JointLimitConstraint:: + return dart::constraint::DynamicJointConstraint:: getConstraintForceMixing(); }); + + ::py::class_< + dart::constraint::BallJointConstraint, + dart::constraint::DynamicJointConstraint, + std::shared_ptr>( + m, "BallJointConstraint") + .def( + ::py::init(), + ::py::arg("body"), + ::py::arg("jointPos")) + .def( + ::py::init< + dart::dynamics::BodyNode*, + dart::dynamics::BodyNode*, + const Eigen::Vector3d&>(), + ::py::arg("body1"), + ::py::arg("body2"), + ::py::arg("jointPos")) + .def_static("getStaticType", +[]() -> std::string { + return dart::constraint::BallJointConstraint::getStaticType(); + }); + + ::py::class_< + dart::constraint::WeldJointConstraint, + dart::constraint::DynamicJointConstraint, + std::shared_ptr>( + m, "WeldJointConstraint") + .def(::py::init(), ::py::arg("body")) + .def( + ::py::init(), + ::py::arg("body1"), + ::py::arg("body2")) + .def_static( + "getStaticType", + +[]() -> std::string { + return dart::constraint::WeldJointConstraint::getStaticType(); + }) + .def( + "setRelativeTransform", + +[](dart::constraint::WeldJointConstraint* self, + const Eigen::Isometry3d& tf) { self->setRelativeTransform(tf); }, + ::py::arg("tf")); } } // namespace python diff --git a/python/dartpy/constraint/JointConstraint.cpp b/python/dartpy/constraint/JointConstraint.cpp index 9b92291edab02..2fb8bdc46fac5 100644 --- a/python/dartpy/constraint/JointConstraint.cpp +++ b/python/dartpy/constraint/JointConstraint.cpp @@ -31,7 +31,6 @@ */ #include -#include #include namespace py = pybind11; @@ -45,6 +44,7 @@ void JointConstraint(py::module& m) dart::constraint::JointConstraint, dart::constraint::ConstraintBase, std::shared_ptr>(m, "JointConstraint") + .def(::py::init(), ::py::arg("joint")) .def_static( "setErrorAllowance", +[](double allowance) { @@ -58,8 +58,8 @@ void JointConstraint(py::module& m) }) .def_static( "setErrorReductionParameter", - +[](double erp) { - dart::constraint::JointConstraint::setErrorReductionParameter(erp); + +[](double _erp) { + dart::constraint::JointConstraint::setErrorReductionParameter(_erp); }, ::py::arg("erp")) .def_static( @@ -70,9 +70,9 @@ void JointConstraint(py::module& m) }) .def_static( "setMaxErrorReductionVelocity", - +[](double _erv) { + +[](double erv) { dart::constraint::JointConstraint::setMaxErrorReductionVelocity( - _erv); + erv); }, ::py::arg("erv")) .def_static( @@ -90,48 +90,6 @@ void JointConstraint(py::module& m) .def_static("getConstraintForceMixing", +[]() -> double { return dart::constraint::JointConstraint::getConstraintForceMixing(); }); - - ::py::class_< - dart::constraint::BallJointConstraint, - dart::constraint::JointConstraint, - std::shared_ptr>( - m, "BallJointConstraint") - .def( - ::py::init(), - ::py::arg("body"), - ::py::arg("jointPos")) - .def( - ::py::init< - dart::dynamics::BodyNode*, - dart::dynamics::BodyNode*, - const Eigen::Vector3d&>(), - ::py::arg("body1"), - ::py::arg("body2"), - ::py::arg("jointPos")) - .def_static("getStaticType", +[]() -> std::string { - return dart::constraint::BallJointConstraint::getStaticType(); - }); - - ::py::class_< - dart::constraint::WeldJointConstraint, - dart::constraint::JointConstraint, - std::shared_ptr>( - m, "WeldJointConstraint") - .def(::py::init(), ::py::arg("body")) - .def( - ::py::init(), - ::py::arg("body1"), - ::py::arg("body2")) - .def_static( - "getStaticType", - +[]() -> std::string { - return dart::constraint::WeldJointConstraint::getStaticType(); - }) - .def( - "setRelativeTransform", - +[](dart::constraint::WeldJointConstraint* self, - const Eigen::Isometry3d& tf) { self->setRelativeTransform(tf); }, - ::py::arg("tf")); } } // namespace python diff --git a/python/dartpy/constraint/module.cpp b/python/dartpy/constraint/module.cpp index f606d338414ad..02b7a2e77efe6 100644 --- a/python/dartpy/constraint/module.cpp +++ b/python/dartpy/constraint/module.cpp @@ -39,8 +39,8 @@ namespace python { void ConstraintBase(py::module& sm); void JointConstraint(py::module& sm); -void JointLimitConstraint(py::module& sm); void JointCoulombFrictionConstraint(py::module& sm); +void DynamicJointConstraint(py::module& sm); void BoxedLcpSolver(py::module& sm); void DantzigBoxedLcpSolver(py::module& sm); @@ -49,18 +49,14 @@ void PgsBoxedLcpSolver(py::module& sm); void ConstraintSolver(py::module& sm); void BoxedLcpConstraintSolver(py::module& sm); -// void ConstraintBase(py::module& sm); -// void ConstraintBase(py::module& sm); -// void ConstraintBase(py::module& sm); - void dart_constraint(py::module& m) { auto sm = m.def_submodule("constraint"); ConstraintBase(sm); JointConstraint(sm); - JointLimitConstraint(sm); JointCoulombFrictionConstraint(sm); + DynamicJointConstraint(sm); BoxedLcpSolver(sm); DantzigBoxedLcpSolver(sm); @@ -68,14 +64,6 @@ void dart_constraint(py::module& m) ConstraintSolver(sm); BoxedLcpConstraintSolver(sm); - - // ConstraintBase(sm); - // ConstraintBase(sm); - // ConstraintBase(sm); - // ConstraintBase(sm); - // ConstraintBase(sm); - // ConstraintBase(sm); - // ConstraintBase(sm); } } // namespace python diff --git a/tutorials/tutorial_collisions/main.cpp b/tutorials/tutorial_collisions/main.cpp index 9a38ad758a727..9b4302cc54f49 100644 --- a/tutorials/tutorial_collisions/main.cpp +++ b/tutorials/tutorial_collisions/main.cpp @@ -225,7 +225,7 @@ class MyWindow : public SimWindow { for (std::size_t i = 0; i < mJointConstraints.size(); ++i) { - const dart::constraint::JointConstraintPtr& constraint + const dart::constraint::DynamicJointConstraintPtr& constraint = mJointConstraints[i]; if (constraint->getBodyNode1()->getSkeleton() == skel @@ -250,7 +250,7 @@ class MyWindow : public SimWindow /// History of the active JointConstraints so that we can properly delete them /// when a Skeleton gets removed - std::vector mJointConstraints; + std::vector mJointConstraints; /// A blueprint Skeleton that we will use to spawn balls SkeletonPtr mOriginalBall; diff --git a/tutorials/tutorial_collisions_finished/main.cpp b/tutorials/tutorial_collisions_finished/main.cpp index 3865bb6fdaacb..e0454e3b7c230 100644 --- a/tutorials/tutorial_collisions_finished/main.cpp +++ b/tutorials/tutorial_collisions_finished/main.cpp @@ -300,7 +300,7 @@ class MyWindow : public dart::gui::glut::SimWindow { for (std::size_t i = 0; i < mJointConstraints.size(); ++i) { - const dart::constraint::JointConstraintPtr& constraint + const dart::constraint::DynamicJointConstraintPtr& constraint = mJointConstraints[i]; if (constraint->getBodyNode1()->getSkeleton() == skel @@ -325,7 +325,7 @@ class MyWindow : public dart::gui::glut::SimWindow /// History of the active JointConstraints so that we can properly delete them /// when a Skeleton gets removed - std::vector mJointConstraints; + std::vector mJointConstraints; /// A blueprint Skeleton that we will use to spawn balls SkeletonPtr mOriginalBall; diff --git a/unittests/regression/CMakeLists.txt b/unittests/regression/CMakeLists.txt index 8b49e431b0f0e..9e4f21c58f05a 100644 --- a/unittests/regression/CMakeLists.txt +++ b/unittests/regression/CMakeLists.txt @@ -2,6 +2,11 @@ dart_add_test("regression" test_Issue000Template test_Issue000Template.cpp) dart_add_test("regression" test_Issue1243 test_Issue1243.cpp) +if(TARGET dart-utils) + dart_add_test("regression" test_Issue1583) + target_link_libraries(test_Issue1583 dart-utils) +endif() + if(TARGET dart-utils-urdf) dart_add_test("regression" test_Issue838) diff --git a/unittests/regression/test_Issue1583.cpp b/unittests/regression/test_Issue1583.cpp new file mode 100644 index 0000000000000..7024c7dc4eccb --- /dev/null +++ b/unittests/regression/test_Issue1583.cpp @@ -0,0 +1,84 @@ +/* + * Copyright (c) 2011-2020, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include +#include + +//======================================================================================== +TEST(Issue1583, ServoJointWithPositionLimits) +{ + const double pos_lb = -0.1; + const double pos_ub = +0.1; + const double vel_desired = 1; + + auto skel = dart::utils::SdfParser::readSkeleton( + "dart://sample/sdf/test/test_issue1583.model"); + ASSERT_NE(skel, nullptr); + + auto world = dart::simulation::World::create(); + world->setGravity(Eigen::Vector3d::Zero()); + world->addSkeleton(skel); + ASSERT_EQ(world->getNumSkeletons(), 1); + + auto* joint = skel->getJoint("j1"); + joint->setPositionLowerLimit(0, pos_lb); + joint->setPositionUpperLimit(0, pos_ub); + joint->setLimitEnforcement(true); + joint->setActuatorType(dart::dynamics::Joint::SERVO); + + EXPECT_DOUBLE_EQ(0, joint->getPosition(0)); + EXPECT_DOUBLE_EQ(0, joint->getVelocity(0)); + EXPECT_DOUBLE_EQ(0, joint->getAcceleration(0)); + + for (std::size_t i = 0; i < 1000; ++i) + { + joint->setCommand(0, vel_desired); + world->step(); + + const double pos = joint->getPosition(0); + const double vel = joint->getVelocity(0); + + EXPECT_LE(pos, pos_ub + 1e-6); + EXPECT_GE(pos, pos_lb - 1e-6); + + if (std::abs(vel - vel_desired) > 1e-6) + { + EXPECT_NEAR(pos, pos_ub, 1e-2); + } + } + + EXPECT_NEAR(pos_ub, joint->getPosition(0), 1e-2); + EXPECT_NEAR(0, joint->getVelocity(0), 1e-6); +}