From 30f096a1a6e2a864283520031a81a8456bfb9298 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 5 Jan 2022 16:55:37 -0800 Subject: [PATCH 01/38] refactor impulses --- dart/constraint/BallJointConstraint.cpp | 2 +- dart/constraint/BallJointConstraint.hpp | 22 +++---- dart/constraint/BoxedLcpConstraintSolver.cpp | 17 +++--- dart/constraint/BoxedLcpConstraintSolver.hpp | 4 +- dart/constraint/ConstraintBase.hpp | 2 +- dart/constraint/ConstraintSolver.cpp | 6 +- dart/constraint/ConstraintSolver.hpp | 4 +- dart/constraint/ContactConstraint.cpp | 2 +- dart/constraint/ContactConstraint.hpp | 2 +- dart/constraint/DantzigLCPSolver.cpp | 4 +- .../JointCoulombFrictionConstraint.cpp | 26 +++++---- .../JointCoulombFrictionConstraint.hpp | 11 ++-- dart/constraint/JointLimitConstraint.cpp | 2 +- dart/constraint/JointLimitConstraint.hpp | 11 ++-- dart/constraint/MimicMotorConstraint.cpp | 7 +-- dart/constraint/MimicMotorConstraint.hpp | 2 +- dart/constraint/PGSLCPSolver.cpp | 4 +- dart/constraint/ServoMotorConstraint.cpp | 23 ++++---- dart/constraint/ServoMotorConstraint.hpp | 11 ++-- dart/constraint/SoftContactConstraint.cpp | 2 +- dart/constraint/SoftContactConstraint.hpp | 14 ++--- dart/constraint/WeldJointConstraint.cpp | 57 ++++++++----------- dart/constraint/WeldJointConstraint.hpp | 11 ++-- .../constraint/BoxedLcpConstraintSolver.cpp | 2 +- .../constraint/ConstraintBase.cpp | 2 +- .../constraint/ConstraintSolver.cpp | 2 +- 26 files changed, 121 insertions(+), 131 deletions(-) diff --git a/dart/constraint/BallJointConstraint.cpp b/dart/constraint/BallJointConstraint.cpp index 5b607645e..a864e0e44 100644 --- a/dart/constraint/BallJointConstraint.cpp +++ b/dart/constraint/BallJointConstraint.cpp @@ -302,7 +302,7 @@ void BallJointConstraint::unexcite() } //============================================================================== -void BallJointConstraint::applyImpulse(s_t* _lambda) +void BallJointConstraint::applyImpulse(Eigen::VectorXs _lambda) { mOldX[0] = _lambda[0]; mOldX[1] = _lambda[1]; diff --git a/dart/constraint/BallJointConstraint.hpp b/dart/constraint/BallJointConstraint.hpp index 09e6a439e..33d752212 100644 --- a/dart/constraint/BallJointConstraint.hpp +++ b/dart/constraint/BallJointConstraint.hpp @@ -35,8 +35,8 @@ #include -#include "dart/math/MathTypes.hpp" #include "dart/constraint/JointConstraint.hpp" +#include "dart/math/MathTypes.hpp" namespace dart { namespace constraint { @@ -49,17 +49,18 @@ class BallJointConstraint : public JointConstraint /// Constructor that takes one body and the joint position in the world frame /// \param[in] _body /// \param[in] _jointPos Joint position expressed in world frame - BallJointConstraint(dynamics::BodyNode* _body, - const Eigen::Vector3s& _jointPos); - + BallJointConstraint( + dynamics::BodyNode* _body, const Eigen::Vector3s& _jointPos); /// Constructor that takes two bodies and the joint position in the frame of /// _body1 /// \param[in] _body1 /// \param[in] _body2 /// \param[in] _jointPos Joint position expressed in world frame - BallJointConstraint(dynamics::BodyNode* _body1, dynamics::BodyNode* _body2, - const Eigen::Vector3s& _jointPos); + BallJointConstraint( + dynamics::BodyNode* _body1, + dynamics::BodyNode* _body2, + const Eigen::Vector3s& _jointPos); /// Destructor virtual ~BallJointConstraint(); @@ -88,7 +89,7 @@ class BallJointConstraint : public JointConstraint void unexcite() override; // Documentation inherited - void applyImpulse(s_t* _lambda) override; + void applyImpulse(Eigen::VectorXs _lambda) override; // Documentation inherited bool isActive() const override; @@ -128,8 +129,7 @@ class BallJointConstraint : public JointConstraint EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} // namespace constraint -} // namespace dart - -#endif // DART_CONSTRAINT_BALLJOINTCONSTRAINT_HPP_ +} // namespace constraint +} // namespace dart +#endif // DART_CONSTRAINT_BALLJOINTCONSTRAINT_HPP_ diff --git a/dart/constraint/BoxedLcpConstraintSolver.cpp b/dart/constraint/BoxedLcpConstraintSolver.cpp index 6cd898a44..d9a234dfd 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.cpp +++ b/dart/constraint/BoxedLcpConstraintSolver.cpp @@ -349,7 +349,7 @@ LcpInputs BoxedLcpConstraintSolver::buildLcpInputs(ConstrainedGroup& group) } //============================================================================== -std::vector BoxedLcpConstraintSolver::solveLcp( +Eigen::MatrixXs BoxedLcpConstraintSolver::solveLcp( LcpInputs lcpInputs, ConstrainedGroup& group) { const std::size_t numConstraints = group.getNumConstraints(); @@ -736,9 +736,7 @@ std::vector BoxedLcpConstraintSolver::solveLcp( } // Initialize the vector of constraint impulses we will eventually return. - // Each ith element of the vector will contain a pointer to the constraint - // impulse to be applied for the ith constraint. - std::vector constraintImpulses; + Eigen::MatrixXs constraintImpulses = Eigen::MatrixXs::Zero(numConstraints, 3); // Collect the final solved constraint impulses to apply per constraint. // TODO(mguo): Make impulse magnitudes all have 3 elements regardless of @@ -748,6 +746,8 @@ std::vector BoxedLcpConstraintSolver::solveLcp( const ConstraintBasePtr& constraint = group.getConstraint(i); if (constraint->isContactConstraint()) { + s_t* lambda = mX.data() + mOffset[i]; + constraintImpulses(i, 0) = lambda[0]; std::shared_ptr contactConstraint = std::static_pointer_cast(constraint); // getContact() returns a const, which is generally what we want, but in @@ -766,12 +766,14 @@ std::vector BoxedLcpConstraintSolver::solveLcp( = contactConstraint->isFrictionOn(); if (contactConstraint->isFrictionOn()) { + constraintImpulses(i, 1) = lambda[1]; + constraintImpulses(i, 2) = lambda[2]; const_cast(&contactConstraint->getContact()) ->lcpResultTangent1 - = mX(mOffset[i] + 1); + = lambda[1]; const_cast(&contactConstraint->getContact()) ->lcpResultTangent2 - = mX(mOffset[i] + 2); + = lambda[2]; const ContactConstraint::TangentBasisMatrix D = contactConstraint->getTangentBasisMatrixODE( contactConstraint->getContact().normal); @@ -783,13 +785,12 @@ std::vector BoxedLcpConstraintSolver::solveLcp( = D.col(1); } } - constraintImpulses.push_back(mX.data() + mOffset[i]); } return constraintImpulses; } //============================================================================== -std::vector BoxedLcpConstraintSolver::solveConstrainedGroup( +Eigen::MatrixXs BoxedLcpConstraintSolver::solveConstrainedGroup( ConstrainedGroup& group) { LcpInputs lcpInputs = buildLcpInputs(group); diff --git a/dart/constraint/BoxedLcpConstraintSolver.hpp b/dart/constraint/BoxedLcpConstraintSolver.hpp index 71d520d88..e515667e2 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.hpp +++ b/dart/constraint/BoxedLcpConstraintSolver.hpp @@ -114,13 +114,13 @@ class BoxedLcpConstraintSolver : public ConstraintSolver virtual void setCachedLCPSolution(Eigen::VectorXs X) override; // Documentation inherited. - std::vector solveConstrainedGroup(ConstrainedGroup& group) override; + Eigen::MatrixXs solveConstrainedGroup(ConstrainedGroup& group) override; /// Build the inputs to the LCP from the constraint group. LcpInputs buildLcpInputs(ConstrainedGroup& group); /// Setup and solve an LCP to enforce the constraints on the ConstrainedGroup. - std::vector solveLcp(LcpInputs lcpInputs, ConstrainedGroup& group); + Eigen::MatrixXs solveLcp(LcpInputs lcpInputs, ConstrainedGroup& group); protected: /// Boxed LCP solver diff --git a/dart/constraint/ConstraintBase.hpp b/dart/constraint/ConstraintBase.hpp index e4fe35ff2..219e8c602 100644 --- a/dart/constraint/ConstraintBase.hpp +++ b/dart/constraint/ConstraintBase.hpp @@ -98,7 +98,7 @@ class ConstraintBase virtual void unexcite() = 0; /// Apply computed constraint impulse to constrained skeletons - virtual void applyImpulse(s_t* lambda) = 0; + virtual void applyImpulse(Eigen::VectorXs lambda) = 0; /// Return true if this constraint is active virtual bool isActive() const = 0; diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 2af247094..cf8e153d8 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -804,20 +804,20 @@ void ConstraintSolver::solveConstrainedGroups() if (0u == n) continue; - std::vector impulses = solveConstrainedGroup(constraintGroup); + Eigen::MatrixXs impulses = solveConstrainedGroup(constraintGroup); applyConstraintImpulses(constraintGroup.getConstraints(), impulses); } } //============================================================================== void ConstraintSolver::applyConstraintImpulses( - std::vector constraints, std::vector impulses) + std::vector constraints, Eigen::MatrixXs impulses) { const std::size_t numConstraints = constraints.size(); for (std::size_t i = 0; i < numConstraints; ++i) { const ConstraintBasePtr& constraint = constraints[i]; - constraint->applyImpulse(impulses[i]); + constraint->applyImpulse(impulses.row(i)); constraint->excite(); } } diff --git a/dart/constraint/ConstraintSolver.hpp b/dart/constraint/ConstraintSolver.hpp index 92b48171f..4f7f0efc1 100644 --- a/dart/constraint/ConstraintSolver.hpp +++ b/dart/constraint/ConstraintSolver.hpp @@ -206,11 +206,11 @@ class ConstraintSolver void solveConstrainedGroups(); // Solve for constraint impulses to apply to each constraint in group. - virtual std::vector solveConstrainedGroup(ConstrainedGroup& group) = 0; + virtual Eigen::MatrixXs solveConstrainedGroup(ConstrainedGroup& group) = 0; /// Apply constraint impulses to each constraint. void applyConstraintImpulses( - std::vector constraints, std::vector impulses); + std::vector constraints, Eigen::MatrixXs impulses); /// Get constrained groups. const std::vector& getConstrainedGroups() const; diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index 8a571473e..1545a2a8d 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -627,7 +627,7 @@ void ContactConstraint::unexcite() } //============================================================================== -void ContactConstraint::applyImpulse(s_t* lambda) +void ContactConstraint::applyImpulse(Eigen::VectorXs lambda) { //---------------------------------------------------------------------------- // Friction case diff --git a/dart/constraint/ContactConstraint.hpp b/dart/constraint/ContactConstraint.hpp index 860bd374c..e6ae20a83 100644 --- a/dart/constraint/ContactConstraint.hpp +++ b/dart/constraint/ContactConstraint.hpp @@ -142,7 +142,7 @@ class ContactConstraint : public ConstraintBase void unexcite() override; // Documentation inherited - void applyImpulse(s_t* lambda) override; + void applyImpulse(Eigen::VectorXs lambda) override; // Documentation inherited dynamics::SkeletonPtr getRootSkeleton() const override; diff --git a/dart/constraint/DantzigLCPSolver.cpp b/dart/constraint/DantzigLCPSolver.cpp index 99f658d81..ff7ef0f34 100644 --- a/dart/constraint/DantzigLCPSolver.cpp +++ b/dart/constraint/DantzigLCPSolver.cpp @@ -218,8 +218,8 @@ void DantzigLCPSolver::solve(ConstrainedGroup* _group) for (std::size_t i = 0; i < numConstraints; ++i) { const ConstraintBasePtr& constraint = _group->getConstraint(i); - constraint->applyImpulse(x + offset[i]); - constraint->excite(); + // constraint->applyImpulse(x + offset[i]); + // constraint->excite(); } delete[] offset; diff --git a/dart/constraint/JointCoulombFrictionConstraint.cpp b/dart/constraint/JointCoulombFrictionConstraint.cpp index 86842cee6..0ab11e658 100644 --- a/dart/constraint/JointCoulombFrictionConstraint.cpp +++ b/dart/constraint/JointCoulombFrictionConstraint.cpp @@ -39,7 +39,7 @@ #include "dart/dynamics/Joint.hpp" #include "dart/dynamics/Skeleton.hpp" -#define DART_CFM 1e-9 +#define DART_CFM 1e-9 namespace dart { namespace constraint { @@ -84,13 +84,15 @@ void JointCoulombFrictionConstraint::setConstraintForceMixing(s_t _cfm) if (_cfm < 1e-9) { dtwarn << "Constraint force mixing parameter[" << _cfm - << "] is lower than 1e-9. " << "It is set to 1e-9." << std::endl; + << "] is lower than 1e-9. " + << "It is set to 1e-9." << std::endl; mConstraintForceMixing = 1e-9; } if (_cfm > 1.0) { dtwarn << "Constraint force mixing parameter[" << _cfm - << "] is greater than 1.0. " << "It is set to 1.0." << std::endl; + << "] is greater than 1.0. " + << "It is set to 1.0." << std::endl; mConstraintForceMixing = 1.0; } @@ -126,7 +128,7 @@ void JointCoulombFrictionConstraint::update() // redundancy. // Note: Coulomb friction is force not impulse - mUpperBound[i] = mJoint->getCoulombFriction(i) * timeStep; + mUpperBound[i] = mJoint->getCoulombFriction(i) * timeStep; mLowerBound[i] = -mUpperBound[i]; if (mActive[i]) @@ -205,14 +207,14 @@ void JointCoulombFrictionConstraint::applyUnitImpulse(std::size_t _index) } //============================================================================== -void JointCoulombFrictionConstraint::getVelocityChange(s_t* _delVel, - bool _withCfm) +void JointCoulombFrictionConstraint::getVelocityChange( + s_t* _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) + for (std::size_t i = 0; i < dof; ++i) { if (mActive[i] == false) continue; @@ -229,8 +231,8 @@ void JointCoulombFrictionConstraint::getVelocityChange(s_t* _delVel, // varaible in ODE if (_withCfm) { - _delVel[mAppliedImpulseIndex] += _delVel[mAppliedImpulseIndex] - * mConstraintForceMixing; + _delVel[mAppliedImpulseIndex] + += _delVel[mAppliedImpulseIndex] * mConstraintForceMixing; } assert(localIndex == mDim); @@ -249,17 +251,17 @@ void JointCoulombFrictionConstraint::unexcite() } //============================================================================== -void JointCoulombFrictionConstraint::applyImpulse(s_t* _lambda) +void JointCoulombFrictionConstraint::applyImpulse(Eigen::VectorXs _lambda) { std::size_t localIndex = 0; std::size_t dof = mJoint->getNumDofs(); - for (std::size_t i = 0; i < dof ; ++i) + for (std::size_t i = 0; i < dof; ++i) { if (mActive[i] == false) continue; mJoint->setConstraintImpulse( - i, mJoint->getConstraintImpulse(i) + _lambda[localIndex]); + i, mJoint->getConstraintImpulse(i) + _lambda[localIndex]); mOldX[i] = _lambda[localIndex]; diff --git a/dart/constraint/JointCoulombFrictionConstraint.hpp b/dart/constraint/JointCoulombFrictionConstraint.hpp index 85a2a40c7..067f9250a 100644 --- a/dart/constraint/JointCoulombFrictionConstraint.hpp +++ b/dart/constraint/JointCoulombFrictionConstraint.hpp @@ -40,7 +40,7 @@ namespace dart { namespace dynamics { class BodyNode; class Joint; -} // namespace dynamics +} // namespace dynamics namespace constraint { @@ -95,7 +95,7 @@ class JointCoulombFrictionConstraint : public ConstraintBase void unexcite() override; // Documentation inherited - void applyImpulse(s_t* _lambda) override; + void applyImpulse(Eigen::VectorXs _lambda) override; // Documentation inherited dynamics::SkeletonPtr getRootSkeleton() const override; @@ -137,8 +137,7 @@ class JointCoulombFrictionConstraint : public ConstraintBase static s_t mConstraintForceMixing; }; -} // namespace constraint -} // namespace dart - -#endif // DART_CONSTRAINT_JOINTCOULOMBFRICTIONCONSTRAINT_HPP_ +} // namespace constraint +} // namespace dart +#endif // DART_CONSTRAINT_JOINTCOULOMBFRICTIONCONSTRAINT_HPP_ diff --git a/dart/constraint/JointLimitConstraint.cpp b/dart/constraint/JointLimitConstraint.cpp index bc785a12e..59da747f2 100644 --- a/dart/constraint/JointLimitConstraint.cpp +++ b/dart/constraint/JointLimitConstraint.cpp @@ -360,7 +360,7 @@ void JointLimitConstraint::unexcite() } //============================================================================== -void JointLimitConstraint::applyImpulse(s_t* _lambda) +void JointLimitConstraint::applyImpulse(Eigen::VectorXs _lambda) { std::size_t localIndex = 0; std::size_t dof = mJoint->getNumDofs(); diff --git a/dart/constraint/JointLimitConstraint.hpp b/dart/constraint/JointLimitConstraint.hpp index 3afac7e42..6e230865e 100644 --- a/dart/constraint/JointLimitConstraint.hpp +++ b/dart/constraint/JointLimitConstraint.hpp @@ -40,7 +40,7 @@ namespace dart { namespace dynamics { class BodyNode; class Joint; -} // namespace dynamics +} // namespace dynamics namespace constraint { @@ -114,7 +114,7 @@ class JointLimitConstraint : public ConstraintBase void unexcite() override; // Documentation inherited - void applyImpulse(s_t* _lambda) override; + void applyImpulse(Eigen::VectorXs _lambda) override; // Documentation inherited dynamics::SkeletonPtr getRootSkeleton() const override; @@ -169,8 +169,7 @@ class JointLimitConstraint : public ConstraintBase static s_t mConstraintForceMixing; }; -} // namespace constraint -} // namespace dart - -#endif // DART_CONSTRAINT_JOINTLIMITCONSTRAINT_HPP_ +} // namespace constraint +} // namespace dart +#endif // DART_CONSTRAINT_JOINTLIMITCONSTRAINT_HPP_ diff --git a/dart/constraint/MimicMotorConstraint.cpp b/dart/constraint/MimicMotorConstraint.cpp index f99793390..16a5ad446 100644 --- a/dart/constraint/MimicMotorConstraint.cpp +++ b/dart/constraint/MimicMotorConstraint.cpp @@ -34,12 +34,11 @@ #include -#include "dart/external/odelcpsolver/lcp.h" - #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_CFM 1e-9 @@ -129,7 +128,7 @@ void MimicMotorConstraint::update() { s_t timeStep = mJoint->getSkeleton()->getTimeStep(); s_t qError = mMimicJoint->getPosition(i) * mMultiplier + mOffset - - mJoint->getPosition(i); + - mJoint->getPosition(i); s_t desiredVelocity = math::clip( qError / timeStep, mJoint->getVelocityLowerLimit(i), @@ -262,7 +261,7 @@ void MimicMotorConstraint::unexcite() } //============================================================================== -void MimicMotorConstraint::applyImpulse(s_t* lambda) +void MimicMotorConstraint::applyImpulse(Eigen::VectorXs lambda) { std::size_t localIndex = 0; std::size_t dof = mJoint->getNumDofs(); diff --git a/dart/constraint/MimicMotorConstraint.hpp b/dart/constraint/MimicMotorConstraint.hpp index 05fd60782..75bf1ebe2 100644 --- a/dart/constraint/MimicMotorConstraint.hpp +++ b/dart/constraint/MimicMotorConstraint.hpp @@ -99,7 +99,7 @@ class MimicMotorConstraint : public ConstraintBase void unexcite() override; // Documentation inherited - void applyImpulse(s_t* lambda) override; + void applyImpulse(Eigen::VectorXs lambda) override; // Documentation inherited dynamics::SkeletonPtr getRootSkeleton() const override; diff --git a/dart/constraint/PGSLCPSolver.cpp b/dart/constraint/PGSLCPSolver.cpp index ef93d639c..52c18d1ac 100644 --- a/dart/constraint/PGSLCPSolver.cpp +++ b/dart/constraint/PGSLCPSolver.cpp @@ -185,8 +185,8 @@ void PGSLCPSolver::solve(ConstrainedGroup* _group) for (std::size_t i = 0; i < numConstraints; ++i) { const ConstraintBasePtr& constraint = _group->getConstraint(i); - constraint->applyImpulse(x + offset[i]); - constraint->excite(); + // constraint->applyImpulse(x + offset[i]); + // constraint->excite(); } delete[] offset; diff --git a/dart/constraint/ServoMotorConstraint.cpp b/dart/constraint/ServoMotorConstraint.cpp index 41ab1bc6d..090bbf356 100644 --- a/dart/constraint/ServoMotorConstraint.cpp +++ b/dart/constraint/ServoMotorConstraint.cpp @@ -34,14 +34,13 @@ #include -#include "dart/external/odelcpsolver/lcp.h" - #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_CFM 1e-9 +#define DART_CFM 1e-9 namespace dart { namespace constraint { @@ -86,14 +85,16 @@ void ServoMotorConstraint::setConstraintForceMixing(s_t cfm) { dtwarn << "[ServoMotorConstraint::setConstraintForceMixing] " << "Constraint force mixing parameter[" << cfm - << "] is lower than 1e-9. " << "It is set to 1e-9." << std::endl; + << "] is lower than 1e-9. " + << "It is set to 1e-9." << std::endl; mConstraintForceMixing = 1e-9; } if (cfm > 1.0) { dtwarn << "[ServoMotorConstraint::setConstraintForceMixing] " << "Constraint force mixing parameter[" << cfm - << "] is greater than 1.0. " << "It is set to 1.0." << std::endl; + << "] is greater than 1.0. " + << "It is set to 1.0." << std::endl; mConstraintForceMixing = 1.0; } @@ -214,7 +215,7 @@ void ServoMotorConstraint::getVelocityChange(s_t* delVel, bool withCfm) std::size_t localIndex = 0; std::size_t dof = mJoint->getNumDofs(); - for (std::size_t i = 0; i < dof ; ++i) + for (std::size_t i = 0; i < dof; ++i) { if (mActive[i] == false) continue; @@ -231,8 +232,8 @@ void ServoMotorConstraint::getVelocityChange(s_t* delVel, bool withCfm) // varaible in ODE if (withCfm) { - delVel[mAppliedImpulseIndex] += delVel[mAppliedImpulseIndex] - * mConstraintForceMixing; + delVel[mAppliedImpulseIndex] + += delVel[mAppliedImpulseIndex] * mConstraintForceMixing; } assert(localIndex == mDim); @@ -251,17 +252,17 @@ void ServoMotorConstraint::unexcite() } //============================================================================== -void ServoMotorConstraint::applyImpulse(s_t* lambda) +void ServoMotorConstraint::applyImpulse(Eigen::VectorXs lambda) { std::size_t localIndex = 0; std::size_t dof = mJoint->getNumDofs(); - for (std::size_t i = 0; i < dof ; ++i) + for (std::size_t i = 0; i < dof; ++i) { if (mActive[i] == false) continue; mJoint->setConstraintImpulse( - i, mJoint->getConstraintImpulse(i) + lambda[localIndex]); + i, mJoint->getConstraintImpulse(i) + lambda[localIndex]); // TODO(JS): consider to add Joint::addConstraintImpulse() mOldX[i] = lambda[localIndex]; diff --git a/dart/constraint/ServoMotorConstraint.hpp b/dart/constraint/ServoMotorConstraint.hpp index f5de7450e..1932affca 100644 --- a/dart/constraint/ServoMotorConstraint.hpp +++ b/dart/constraint/ServoMotorConstraint.hpp @@ -40,7 +40,7 @@ namespace dart { namespace dynamics { class BodyNode; class Joint; -} // namespace dynamics +} // namespace dynamics namespace constraint { @@ -95,7 +95,7 @@ class ServoMotorConstraint : public ConstraintBase void unexcite() override; // Documentation inherited - void applyImpulse(s_t* lambda) override; + void applyImpulse(Eigen::VectorXs lambda) override; // Documentation inherited dynamics::SkeletonPtr getRootSkeleton() const override; @@ -139,8 +139,7 @@ class ServoMotorConstraint : public ConstraintBase static s_t mConstraintForceMixing; }; -} // namespace constraint -} // namespace dart - -#endif // DART_CONSTRAINT_SERVOMOTORCONSTRAINT_HPP_ +} // namespace constraint +} // namespace dart +#endif // DART_CONSTRAINT_SERVOMOTORCONSTRAINT_HPP_ diff --git a/dart/constraint/SoftContactConstraint.cpp b/dart/constraint/SoftContactConstraint.cpp index b407c378e..cd00d9f61 100644 --- a/dart/constraint/SoftContactConstraint.cpp +++ b/dart/constraint/SoftContactConstraint.cpp @@ -733,7 +733,7 @@ void SoftContactConstraint::unexcite() } //============================================================================== -void SoftContactConstraint::applyImpulse(s_t* _lambda) +void SoftContactConstraint::applyImpulse(Eigen::VectorXs _lambda) { // TODO(JS): Need to optimize code diff --git a/dart/constraint/SoftContactConstraint.hpp b/dart/constraint/SoftContactConstraint.hpp index 0a9d4f188..f7d5f8dc4 100644 --- a/dart/constraint/SoftContactConstraint.hpp +++ b/dart/constraint/SoftContactConstraint.hpp @@ -33,23 +33,22 @@ #ifndef DART_CONSTRAINT_SOFTCONTACTCONSTRAINT_HPP_ #define DART_CONSTRAINT_SOFTCONTACTCONSTRAINT_HPP_ +#include "dart/collision/CollisionDetector.hpp" #include "dart/constraint/ConstraintBase.hpp" - #include "dart/math/MathTypes.hpp" -#include "dart/collision/CollisionDetector.hpp" namespace dart { -namespace collision{ +namespace collision { class SoftCollisionInfo; -} // namespace collision +} // namespace collision namespace dynamics { class BodyNode; class SoftBodyNode; class PointMass; class Skeleton; -} // namespace dynamics +} // namespace dynamics namespace constraint { @@ -128,7 +127,7 @@ class SoftContactConstraint : public ConstraintBase void unexcite() override; // Documentation inherited - void applyImpulse(s_t* _lambda) override; + void applyImpulse(Eigen::VectorXs _lambda) override; // Documentation inherited dynamics::SkeletonPtr getRootSkeleton() const override; @@ -247,5 +246,4 @@ class SoftContactConstraint : public ConstraintBase } // namespace constraint } // namespace dart -#endif // DART_CONSTRAINT_SOFTCONTACTCONSTRAINT_HPP_ - +#endif // DART_CONSTRAINT_SOFTCONTACTCONSTRAINT_HPP_ diff --git a/dart/constraint/WeldJointConstraint.cpp b/dart/constraint/WeldJointConstraint.cpp index f7aa1fe5e..c6365523a 100644 --- a/dart/constraint/WeldJointConstraint.cpp +++ b/dart/constraint/WeldJointConstraint.cpp @@ -32,10 +32,9 @@ #include "dart/constraint/WeldJointConstraint.hpp" -#include "dart/external/odelcpsolver/lcp.h" - #include "dart/dynamics/BodyNode.hpp" #include "dart/dynamics/Skeleton.hpp" +#include "dart/external/odelcpsolver/lcp.h" namespace dart { namespace constraint { @@ -59,11 +58,11 @@ WeldJointConstraint::WeldJointConstraint(dynamics::BodyNode* _body) } //============================================================================== -WeldJointConstraint::WeldJointConstraint(dynamics::BodyNode* _body1, - dynamics::BodyNode* _body2) +WeldJointConstraint::WeldJointConstraint( + dynamics::BodyNode* _body1, dynamics::BodyNode* _body2) : JointConstraint(_body1, _body2), - mRelativeTransform(_body2->getTransform().inverse() - * _body1->getTransform()), + mRelativeTransform( + _body2->getTransform().inverse() * _body1->getTransform()), mViolation(Eigen::Vector6s::Zero()), mJacobian1(Eigen::Matrix6s::Identity()), mAppliedImpulseIndex(0) @@ -108,18 +107,17 @@ void WeldJointConstraint::update() // Update Jacobian for body2 if (mBodyNode2) { - Eigen::Isometry3s T12 = mBodyNode1->getTransform().inverse() - * mBodyNode2->getTransform(); + Eigen::Isometry3s T12 + = mBodyNode1->getTransform().inverse() * mBodyNode2->getTransform(); mJacobian2 = math::AdTJac(T12, mJacobian1); } // Update position constraint error if (mBodyNode2) { - const Eigen::Isometry3s& violationT - = mRelativeTransform.inverse() - * mBodyNode2->getTransform().inverse() - * mBodyNode1->getTransform(); + const Eigen::Isometry3s& violationT = mRelativeTransform.inverse() + * mBodyNode2->getTransform().inverse() + * mBodyNode1->getTransform(); mViolation = math::logMap(violationT); } @@ -206,13 +204,15 @@ void WeldJointConstraint::applyUnitImpulse(std::size_t _index) if (mBodyNode2->isReactive()) { mBodyNode1->getSkeleton()->updateBiasImpulse( - mBodyNode1, mJacobian1.row(_index), - mBodyNode2, -mJacobian2.row(_index)); + mBodyNode1, + mJacobian1.row(_index), + mBodyNode2, + -mJacobian2.row(_index)); } else { mBodyNode1->getSkeleton()->updateBiasImpulse( - mBodyNode1, mJacobian1.row(_index)); + mBodyNode1, mJacobian1.row(_index)); } } else @@ -220,7 +220,7 @@ void WeldJointConstraint::applyUnitImpulse(std::size_t _index) if (mBodyNode2->isReactive()) { mBodyNode2->getSkeleton()->updateBiasImpulse( - mBodyNode2, -mJacobian2.row(_index)); + mBodyNode2, -mJacobian2.row(_index)); } else { @@ -237,7 +237,7 @@ void WeldJointConstraint::applyUnitImpulse(std::size_t _index) { mBodyNode1->getSkeleton()->clearConstraintImpulses(); mBodyNode1->getSkeleton()->updateBiasImpulse( - mBodyNode1, mJacobian1.row(_index)); + mBodyNode1, mJacobian1.row(_index)); mBodyNode1->getSkeleton()->updateVelocityChange(); } @@ -245,7 +245,7 @@ void WeldJointConstraint::applyUnitImpulse(std::size_t _index) { mBodyNode2->getSkeleton()->clearConstraintImpulses(); mBodyNode2->getSkeleton()->updateBiasImpulse( - mBodyNode2, -mJacobian2.row(_index)); + mBodyNode2, -mJacobian2.row(_index)); mBodyNode2->getSkeleton()->updateVelocityChange(); } } @@ -256,7 +256,7 @@ void WeldJointConstraint::applyUnitImpulse(std::size_t _index) mBodyNode1->getSkeleton()->clearConstraintImpulses(); mBodyNode1->getSkeleton()->updateBiasImpulse( - mBodyNode1, mJacobian1.row(_index)); + mBodyNode1, mJacobian1.row(_index)); mBodyNode1->getSkeleton()->updateVelocityChange(); } @@ -270,14 +270,12 @@ void WeldJointConstraint::getVelocityChange(s_t* _vel, bool _withCfm) assert(isActive()); Eigen::Vector6s velChange = Eigen::Vector6s::Zero(); - if (mBodyNode1->getSkeleton()->isImpulseApplied() - && mBodyNode1->isReactive()) + if (mBodyNode1->getSkeleton()->isImpulseApplied() && mBodyNode1->isReactive()) { velChange += mBodyNode1->getBodyVelocityChange(); } - if (mBodyNode2 - && mBodyNode2->getSkeleton()->isImpulseApplied() + if (mBodyNode2 && mBodyNode2->getSkeleton()->isImpulseApplied() && mBodyNode2->isReactive()) { velChange -= mJacobian2 * mBodyNode2->getBodyVelocityChange(); @@ -290,8 +288,8 @@ void WeldJointConstraint::getVelocityChange(s_t* _vel, bool _withCfm) // varaible in ODE if (_withCfm) { - _vel[mAppliedImpulseIndex] += _vel[mAppliedImpulseIndex] - * mConstraintForceMixing; + _vel[mAppliedImpulseIndex] + += _vel[mAppliedImpulseIndex] * mConstraintForceMixing; } } @@ -322,7 +320,7 @@ void WeldJointConstraint::unexcite() } //============================================================================== -void WeldJointConstraint::applyImpulse(s_t* _lambda) +void WeldJointConstraint::applyImpulse(Eigen::VectorXs _lambda) { mOldX[0] = _lambda[0]; mOldX[1] = _lambda[1]; @@ -332,12 +330,7 @@ void WeldJointConstraint::applyImpulse(s_t* _lambda) mOldX[5] = _lambda[5]; Eigen::Vector6s imp; - imp << _lambda[0], - _lambda[1], - _lambda[2], - _lambda[3], - _lambda[4], - _lambda[5]; + imp << _lambda[0], _lambda[1], _lambda[2], _lambda[3], _lambda[4], _lambda[5]; mBodyNode1->addConstraintImpulse(imp); diff --git a/dart/constraint/WeldJointConstraint.hpp b/dart/constraint/WeldJointConstraint.hpp index 77ea6f178..5cfd7ebff 100644 --- a/dart/constraint/WeldJointConstraint.hpp +++ b/dart/constraint/WeldJointConstraint.hpp @@ -35,8 +35,8 @@ #include -#include "dart/math/MathTypes.hpp" #include "dart/constraint/JointConstraint.hpp" +#include "dart/math/MathTypes.hpp" namespace dart { namespace constraint { @@ -85,7 +85,7 @@ class WeldJointConstraint : public JointConstraint void unexcite() override; // Documentation inherited - void applyImpulse(s_t* _lambda) override; + void applyImpulse(Eigen::VectorXs _lambda) override; // Documentation inherited bool isActive() const override; @@ -120,8 +120,7 @@ class WeldJointConstraint : public JointConstraint EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} // namespace constraint -} // namespace dart - -#endif // DART_CONSTRAINT_WELDJOINTCONSTRAINT_HPP_ +} // namespace constraint +} // namespace dart +#endif // DART_CONSTRAINT_WELDJOINTCONSTRAINT_HPP_ diff --git a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp index fa62e2f81..5918284c1 100644 --- a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp @@ -77,7 +77,7 @@ void BoxedLcpConstraintSolver(py::module& m) "solveLcp", +[](dart::constraint::BoxedLcpConstraintSolver* self, dart::constraint::LcpInputs lcpInputs, - dart::constraint::ConstrainedGroup& group) -> std::vector { + dart::constraint::ConstrainedGroup& group) -> Eigen::MatrixXs { return self->solveLcp(lcpInputs, group); }); } diff --git a/python/_nimblephysics/constraint/ConstraintBase.cpp b/python/_nimblephysics/constraint/ConstraintBase.cpp index 7408ebe12..23d0f2bde 100644 --- a/python/_nimblephysics/constraint/ConstraintBase.cpp +++ b/python/_nimblephysics/constraint/ConstraintBase.cpp @@ -81,7 +81,7 @@ void ConstraintBase(py::module& m) +[](dart::constraint::ConstraintBase* self) { self->unexcite(); }) .def( "applyImpulse", - +[](dart::constraint::ConstraintBase* self, s_t* lambda) { + +[](dart::constraint::ConstraintBase* self, Eigen::VectorXs lambda) { self->applyImpulse(lambda); }, ::py::arg("lambda")) diff --git a/python/_nimblephysics/constraint/ConstraintSolver.cpp b/python/_nimblephysics/constraint/ConstraintSolver.cpp index 757c0fa09..e9d404d32 100644 --- a/python/_nimblephysics/constraint/ConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/ConstraintSolver.cpp @@ -201,7 +201,7 @@ void ConstraintSolver(py::module& m) "applyConstraintImpulses", +[](dart::constraint::ConstraintSolver* self, std::vector constraints, - std::vector impulses) { + Eigen::VectorXs impulses) { self->applyConstraintImpulses(constraints, impulses); }) .def( From 1aa1f0123606ab0e5ff68ab306b686040f724004 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 5 Jan 2022 17:08:21 -0800 Subject: [PATCH 02/38] add eigen import --- python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp index 5918284c1..18424efca 100644 --- a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include From f415caf7b8b53749bfd95ff5240e46c0a9d6adb9 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 5 Jan 2022 17:12:33 -0800 Subject: [PATCH 03/38] one more eigen import --- python/_nimblephysics/constraint/ConstraintSolver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/python/_nimblephysics/constraint/ConstraintSolver.cpp b/python/_nimblephysics/constraint/ConstraintSolver.cpp index e9d404d32..c822092f6 100644 --- a/python/_nimblephysics/constraint/ConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/ConstraintSolver.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include #include #include From 6578761565122ab18525a68fecc0490015f3dc2a Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 5 Jan 2022 18:00:33 -0800 Subject: [PATCH 04/38] matrix instead of vector --- python/_nimblephysics/constraint/ConstraintSolver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/_nimblephysics/constraint/ConstraintSolver.cpp b/python/_nimblephysics/constraint/ConstraintSolver.cpp index c822092f6..22d7b5133 100644 --- a/python/_nimblephysics/constraint/ConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/ConstraintSolver.cpp @@ -202,7 +202,7 @@ void ConstraintSolver(py::module& m) "applyConstraintImpulses", +[](dart::constraint::ConstraintSolver* self, std::vector constraints, - Eigen::VectorXs impulses) { + Eigen::MatrixXs impulses) { self->applyConstraintImpulses(constraints, impulses); }) .def( From c12130dfaeed9f4763e37086f438a42e4f14b79a Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 5 Jan 2022 19:54:41 -0800 Subject: [PATCH 05/38] fix variable type for unittest --- unittests/unit/test_ConstraintSolver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/unittests/unit/test_ConstraintSolver.cpp b/unittests/unit/test_ConstraintSolver.cpp index ab0cd3e52..b17d2c405 100644 --- a/unittests/unit/test_ConstraintSolver.cpp +++ b/unittests/unit/test_ConstraintSolver.cpp @@ -70,7 +70,7 @@ TEST(ConstraintSolver, SIMPLE) // Solve constraint impulses. for (auto constraintGroup : solver->getConstrainedGroups()) { - std::vector impulses = solver->solveConstrainedGroup(constraintGroup); + Eigen::MatrixXs impulses = solver->solveConstrainedGroup(constraintGroup); EXPECT_TRUE(impulses.size() > 0); solver->applyConstraintImpulses(constraintGroup.getConstraints(), impulses); } From 3b3a55cc071a01be923175da10c7089487d70d85 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 22 Mar 2022 12:40:07 -0700 Subject: [PATCH 06/38] Create LCPUtils.cpp under python/_nimblephysics/constraint. --- python/_nimblephysics/constraint/LCPUtils.cpp | 48 +++++++++++++++++++ 1 file changed, 48 insertions(+) create mode 100644 python/_nimblephysics/constraint/LCPUtils.cpp diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp new file mode 100644 index 000000000..49d7692a1 --- /dev/null +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -0,0 +1,48 @@ +/* + * Copyright (c) 2011-2019, 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 + +namespace py = pybind11; + +namespace dart { +namespace python { + +void LCPUtils(py::module& m) +{ + ::py::class_(m, "LCPUtils"); +} + +} // namespace python +} // namespace dart From baa48e436616a77e22791fb946d43a316a2221fc Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 22 Mar 2022 12:44:14 -0700 Subject: [PATCH 07/38] fix typo --- python/_nimblephysics/constraint/LCPUtils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index 49d7692a1..4c9f477a6 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -41,7 +41,7 @@ namespace python { void LCPUtils(py::module& m) { - ::py::class_(m, "LCPUtils"); + ::py::class_(m, "LCPUtils"); } } // namespace python From 76062e44a2d15a6969ec1e81ca71bf38fecc4e21 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 22 Mar 2022 12:50:35 -0700 Subject: [PATCH 08/38] add lcp utils to module.cpp --- python/_nimblephysics/constraint/module.cpp | 14 ++------------ 1 file changed, 2 insertions(+), 12 deletions(-) diff --git a/python/_nimblephysics/constraint/module.cpp b/python/_nimblephysics/constraint/module.cpp index 825508bad..3b87d0f72 100644 --- a/python/_nimblephysics/constraint/module.cpp +++ b/python/_nimblephysics/constraint/module.cpp @@ -51,10 +51,7 @@ void BoxedLcpConstraintSolver(py::module& sm); void ConstrainedGroup(py::module& sm); void LcpInputs(py::module& sm); - -// void ConstraintBase(py::module& sm); -// void ConstraintBase(py::module& sm); -// void ConstraintBase(py::module& sm); +void LCPUtils(py::module& sm); void dart_constraint(py::module& m) { @@ -74,14 +71,7 @@ void dart_constraint(py::module& m) ConstrainedGroup(sm); LcpInputs(sm); - - // ConstraintBase(sm); - // ConstraintBase(sm); - // ConstraintBase(sm); - // ConstraintBase(sm); - // ConstraintBase(sm); - // ConstraintBase(sm); - // ConstraintBase(sm); + LCPUtils(sm); } } // namespace python From 64d7d0f0f408641b8c9c547485815f18558ae7e9 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 22 Mar 2022 12:59:02 -0700 Subject: [PATCH 09/38] import missing header file --- python/_nimblephysics/constraint/LCPUtils.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index 4c9f477a6..625ff9382 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -31,6 +31,7 @@ */ #include +#include #include #include @@ -41,7 +42,9 @@ namespace python { void LCPUtils(py::module& m) { - ::py::class_(m, "LCPUtils"); + ::py::class_< + dart::constraint::LCPUtils, + std::shared_ptr >(m, "LCPUtils"); } } // namespace python From cec39068ccc9981ee9f6b3cb7ca601ae7bc338fb Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 22 Mar 2022 13:15:14 -0700 Subject: [PATCH 10/38] add binding for isLcpSolutionValid --- python/_nimblephysics/constraint/LCPUtils.cpp | 22 ++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index 625ff9382..c6ffb38a7 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -44,7 +44,27 @@ void LCPUtils(py::module& m) { ::py::class_< dart::constraint::LCPUtils, - std::shared_ptr >(m, "LCPUtils"); + std::shared_ptr >(m, "LCPUtils") + .def( + "isLcpSolutionValid", + +[](dart::constraint::LCPUtils* self, + const Eigen::MatrixXs& mA, + const Eigen::VectorXs& mX, + const Eigen::VectorXs& mB, + const Eigen::VectorXs& mHi, + const Eigen::VectorXs& mLo, + const Eigen::VectorXi& mFIndex, + bool ignoreFrictionIndices) { + self->isLcpSolutionValid( + mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); + }, + ::py::arg("A"), + ::py::arg("x"), + ::py::arg("b"), + ::py::arg("hi"), + ::py::arg("lo"), + ::py::arg("fIndex"), + ::py::arg("ignoreFrictionIndices")); } } // namespace python From b2ef95a468982b8de67bd8d4da7d959d09f5b72d Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 22 Mar 2022 13:16:49 -0700 Subject: [PATCH 11/38] fix typo --- python/_nimblephysics/constraint/LCPUtils.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index c6ffb38a7..cb84026da 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -46,7 +46,7 @@ void LCPUtils(py::module& m) dart::constraint::LCPUtils, std::shared_ptr >(m, "LCPUtils") .def( - "isLcpSolutionValid", + "isLCPSolutionValid", +[](dart::constraint::LCPUtils* self, const Eigen::MatrixXs& mA, const Eigen::VectorXs& mX, @@ -55,7 +55,7 @@ void LCPUtils(py::module& m) const Eigen::VectorXs& mLo, const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices) { - self->isLcpSolutionValid( + self->isLCPSolutionValid( mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); }, ::py::arg("A"), From 27635b4a740eeac7d74f8c0e32eac08b5343e175 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 22 Mar 2022 16:46:57 -0700 Subject: [PATCH 12/38] add default args and return type --- python/_nimblephysics/constraint/LCPUtils.cpp | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index cb84026da..d808eaf6c 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -48,23 +48,23 @@ void LCPUtils(py::module& m) .def( "isLCPSolutionValid", +[](dart::constraint::LCPUtils* self, - const Eigen::MatrixXs& mA, - const Eigen::VectorXs& mX, - const Eigen::VectorXs& mB, - const Eigen::VectorXs& mHi, - const Eigen::VectorXs& mLo, - const Eigen::VectorXi& mFIndex, - bool ignoreFrictionIndices) { - self->isLCPSolutionValid( + Eigen::MatrixXs mA, + Eigen::VectorXs mX, + Eigen::VectorXs mB, + Eigen::VectorXs mHi, + Eigen::VectorXs mLo, + Eigen::VectorXi mFIndex, + bool ignoreFrictionIndices) -> bool { + return self->isLCPSolutionValid( mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); }, - ::py::arg("A"), - ::py::arg("x"), - ::py::arg("b"), - ::py::arg("hi"), - ::py::arg("lo"), - ::py::arg("fIndex"), - ::py::arg("ignoreFrictionIndices")); + ::py::arg("A") = Eigen::MatrixXs::Zero(3, 4), + ::py::arg("x") = Eigen::VectorXs::Zero(3), + ::py::arg("b") = Eigen::VectorXs::Zero(3), + ::py::arg("hi") = Eigen::VectorXs::Zero(3), + ::py::arg("lo") = Eigen::VectorXs::Zero(3), + ::py::arg("fIndex") = Eigen::VectorXi::Zero(3), + ::py::arg("ignoreFrictionIndices") = true); } } // namespace python From 73c97f1f40b42f4439839596111807ba548cb0be Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 22 Mar 2022 17:17:06 -0700 Subject: [PATCH 13/38] get isLcpSolutionValid() to run in python without errors --- python/_nimblephysics/constraint/LCPUtils.cpp | 33 ++++++------------- 1 file changed, 10 insertions(+), 23 deletions(-) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index d808eaf6c..259674071 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -42,29 +42,16 @@ namespace python { void LCPUtils(py::module& m) { - ::py::class_< - dart::constraint::LCPUtils, - std::shared_ptr >(m, "LCPUtils") - .def( - "isLCPSolutionValid", - +[](dart::constraint::LCPUtils* self, - Eigen::MatrixXs mA, - Eigen::VectorXs mX, - Eigen::VectorXs mB, - Eigen::VectorXs mHi, - Eigen::VectorXs mLo, - Eigen::VectorXi mFIndex, - bool ignoreFrictionIndices) -> bool { - return self->isLCPSolutionValid( - mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); - }, - ::py::arg("A") = Eigen::MatrixXs::Zero(3, 4), - ::py::arg("x") = Eigen::VectorXs::Zero(3), - ::py::arg("b") = Eigen::VectorXs::Zero(3), - ::py::arg("hi") = Eigen::VectorXs::Zero(3), - ::py::arg("lo") = Eigen::VectorXs::Zero(3), - ::py::arg("fIndex") = Eigen::VectorXi::Zero(3), - ::py::arg("ignoreFrictionIndices") = true); + m.def( + "isLCPSolutionValid", + &dart::constraint::LCPUtils::isLCPSolutionValid, + ::py::arg("mA"), + ::py::arg("mX"), + ::py::arg("mB"), + ::py::arg("mHi"), + ::py::arg("mLo"), + ::py::arg("mFIndex"), + ::py::arg("ignoreFrictionIndices")); } } // namespace python From a3608e558046107a36e4af4d70b551401f8f48d4 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 23 Mar 2022 17:46:10 -0700 Subject: [PATCH 14/38] create and call empty getLCPSolutionType function --- dart/constraint/LCPUtils.cpp | 9 +++++++++ dart/constraint/LCPUtils.hpp | 4 ++++ 2 files changed, 13 insertions(+) diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp index 5ec9601b6..0e9e269c9 100644 --- a/dart/constraint/LCPUtils.cpp +++ b/dart/constraint/LCPUtils.cpp @@ -9,6 +9,7 @@ namespace dart { namespace constraint { //============================================================================== +/// This checks whether a solution to an LCP problem is valid. bool LCPUtils::isLCPSolutionValid( const Eigen::MatrixXs& mA, const Eigen::VectorXs& mX, @@ -18,6 +19,7 @@ bool LCPUtils::isLCPSolutionValid( const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices) { + getLCPSolutionType(); Eigen::VectorXs v = mA * mX - mB; for (int i = 0; i < mX.size(); i++) { @@ -79,6 +81,13 @@ bool LCPUtils::isLCPSolutionValid( return true; } +//============================================================================== +/// This checks whether a solution to an LCP problem is valid. +void LCPUtils::getLCPSolutionType() +{ + // Doesn't do anything, yet. +} + //============================================================================== /// This applies a simple algorithm to guess the solution to the LCP problem. /// It's not guaranteed to be correct, but it often can be if there is no diff --git a/dart/constraint/LCPUtils.hpp b/dart/constraint/LCPUtils.hpp index 1cfe3ec97..c6015c4fa 100644 --- a/dart/constraint/LCPUtils.hpp +++ b/dart/constraint/LCPUtils.hpp @@ -13,6 +13,7 @@ namespace constraint { class LCPUtils { public: + // This checks whether a solution to an LCP problem is valid. static bool isLCPSolutionValid( const Eigen::MatrixXs& mA, const Eigen::VectorXs& mX, @@ -22,6 +23,9 @@ class LCPUtils const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices); + // This determines the type of a solution to an LCP problem. + static void getLCPSolutionType(); + /// This applies a simple algorithm to guess the solution to the LCP problem. /// It's not guaranteed to be correct, but it often can be if there is no /// sliding friction on this timestep. From 1eff5eec519362f0321eb60809521d3542bf7bc2 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 23 Mar 2022 18:06:17 -0700 Subject: [PATCH 15/38] add LcpSolutionType enum --- dart/constraint/LCPUtils.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/dart/constraint/LCPUtils.hpp b/dart/constraint/LCPUtils.hpp index c6015c4fa..990ec9219 100644 --- a/dart/constraint/LCPUtils.hpp +++ b/dart/constraint/LCPUtils.hpp @@ -10,6 +10,12 @@ namespace dart { namespace constraint { +enum LcpSolutionType +{ + SUCCESS, + FAILURE +}; + class LCPUtils { public: From 19e6daece7080f63dd5879b4203681aae49fdaa8 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 23 Mar 2022 18:17:08 -0700 Subject: [PATCH 16/38] make getLCPSolutionType return a LCPSolutionType --- dart/constraint/LCPUtils.cpp | 4 ++-- dart/constraint/LCPUtils.hpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp index 0e9e269c9..a60fe20f7 100644 --- a/dart/constraint/LCPUtils.cpp +++ b/dart/constraint/LCPUtils.cpp @@ -83,9 +83,9 @@ bool LCPUtils::isLCPSolutionValid( //============================================================================== /// This checks whether a solution to an LCP problem is valid. -void LCPUtils::getLCPSolutionType() +LCPSolutionType LCPUtils::getLCPSolutionType() { - // Doesn't do anything, yet. + return LCPSolutionType::FAILURE; } //============================================================================== diff --git a/dart/constraint/LCPUtils.hpp b/dart/constraint/LCPUtils.hpp index 990ec9219..b2f2c1517 100644 --- a/dart/constraint/LCPUtils.hpp +++ b/dart/constraint/LCPUtils.hpp @@ -10,7 +10,7 @@ namespace dart { namespace constraint { -enum LcpSolutionType +enum LCPSolutionType { SUCCESS, FAILURE @@ -30,7 +30,7 @@ class LCPUtils bool ignoreFrictionIndices); // This determines the type of a solution to an LCP problem. - static void getLCPSolutionType(); + static LCPSolutionType getLCPSolutionType(); /// This applies a simple algorithm to guess the solution to the LCP problem. /// It's not guaranteed to be correct, but it often can be if there is no From 77658e94f640a89052ec3d63f769f1239cb37ed7 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 23 Mar 2022 18:22:21 -0700 Subject: [PATCH 17/38] add python binding for getLCPSolutionType --- python/_nimblephysics/constraint/LCPUtils.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index 259674071..a74368fe5 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -52,6 +52,7 @@ void LCPUtils(py::module& m) ::py::arg("mLo"), ::py::arg("mFIndex"), ::py::arg("ignoreFrictionIndices")); + m.def("getLCPSolutionType", &dart::constraint::LCPUtils::getLCPSolutionType); } } // namespace python From 4bfca7253692cdc1a4b56febd0a8f219a752421b Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 23 Mar 2022 18:34:09 -0700 Subject: [PATCH 18/38] add python binding for LCPSolutionType enum --- python/_nimblephysics/constraint/LCPUtils.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index a74368fe5..ae9250315 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -53,6 +53,10 @@ void LCPUtils(py::module& m) ::py::arg("mFIndex"), ::py::arg("ignoreFrictionIndices")); m.def("getLCPSolutionType", &dart::constraint::LCPUtils::getLCPSolutionType); + ::py::enum_(m, "LCPSolutionType") + .value("SUCCESS", dart::constraint::LCPSolutionType::SUCCESS) + .value("FAILURE", dart::constraint::LCPSolutionType::FAILURE) + .export_values(); } } // namespace python From 3537a16e6565d0f882e7bb0e9e34b0f094a1adc9 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 23 Mar 2022 18:47:55 -0700 Subject: [PATCH 19/38] add arguments for getLCPSolutionType --- dart/constraint/LCPUtils.cpp | 18 ++++++++++++++++-- dart/constraint/LCPUtils.hpp | 9 ++++++++- python/_nimblephysics/constraint/LCPUtils.cpp | 11 ++++++++++- 3 files changed, 34 insertions(+), 4 deletions(-) diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp index a60fe20f7..d8f788c2a 100644 --- a/dart/constraint/LCPUtils.cpp +++ b/dart/constraint/LCPUtils.cpp @@ -19,7 +19,7 @@ bool LCPUtils::isLCPSolutionValid( const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices) { - getLCPSolutionType(); + getLCPSolutionType(mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); Eigen::VectorXs v = mA * mX - mB; for (int i = 0; i < mX.size(); i++) { @@ -83,8 +83,22 @@ bool LCPUtils::isLCPSolutionValid( //============================================================================== /// This checks whether a solution to an LCP problem is valid. -LCPSolutionType LCPUtils::getLCPSolutionType() +LCPSolutionType LCPUtils::getLCPSolutionType( + const Eigen::MatrixXs& mA, + const Eigen::VectorXs& mX, + const Eigen::VectorXs& mB, + const Eigen::VectorXs& mHi, + const Eigen::VectorXs& mLo, + const Eigen::VectorXi& mFIndex, + bool ignoreFrictionIndices) { + Eigen::MatrixXs mANew = mA; + Eigen::VectorXs mXNew = mX; + Eigen::VectorXs mBNew = mB; + Eigen::VectorXs mHiNew = mHi; + Eigen::VectorXs mLoNew = mLo; + Eigen::VectorXi mFIndexNew = mFIndex; + ignoreFrictionIndices = !ignoreFrictionIndices; return LCPSolutionType::FAILURE; } diff --git a/dart/constraint/LCPUtils.hpp b/dart/constraint/LCPUtils.hpp index b2f2c1517..5927790fc 100644 --- a/dart/constraint/LCPUtils.hpp +++ b/dart/constraint/LCPUtils.hpp @@ -30,7 +30,14 @@ class LCPUtils bool ignoreFrictionIndices); // This determines the type of a solution to an LCP problem. - static LCPSolutionType getLCPSolutionType(); + static LCPSolutionType getLCPSolutionType( + const Eigen::MatrixXs& mA, + const Eigen::VectorXs& mX, + const Eigen::VectorXs& mB, + const Eigen::VectorXs& mHi, + const Eigen::VectorXs& mLo, + const Eigen::VectorXi& mFIndex, + bool ignoreFrictionIndices); /// This applies a simple algorithm to guess the solution to the LCP problem. /// It's not guaranteed to be correct, but it often can be if there is no diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index ae9250315..5c3505cdc 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -52,7 +52,16 @@ void LCPUtils(py::module& m) ::py::arg("mLo"), ::py::arg("mFIndex"), ::py::arg("ignoreFrictionIndices")); - m.def("getLCPSolutionType", &dart::constraint::LCPUtils::getLCPSolutionType); + m.def( + "getLCPSolutionType", + &dart::constraint::LCPUtils::getLCPSolutionType, + ::py::arg("mA"), + ::py::arg("mX"), + ::py::arg("mB"), + ::py::arg("mHi"), + ::py::arg("mLo"), + ::py::arg("mFIndex"), + ::py::arg("ignoreFrictionIndices")); ::py::enum_(m, "LCPSolutionType") .value("SUCCESS", dart::constraint::LCPSolutionType::SUCCESS) .value("FAILURE", dart::constraint::LCPSolutionType::FAILURE) From 7bb0e68a7b20dd75fed5d707f3bd334b152ef710 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 23 Mar 2022 18:56:20 -0700 Subject: [PATCH 20/38] copy function body, assign return value to a variable --- dart/constraint/LCPUtils.cpp | 71 +++++++++++++++++++++++++++++++----- 1 file changed, 62 insertions(+), 9 deletions(-) diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp index d8f788c2a..90282868b 100644 --- a/dart/constraint/LCPUtils.cpp +++ b/dart/constraint/LCPUtils.cpp @@ -19,7 +19,9 @@ bool LCPUtils::isLCPSolutionValid( const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices) { - getLCPSolutionType(mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); + LCPSolutionType solutionType = getLCPSolutionType( + mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); + (void)solutionType; // temporarily suppress unused variable warnings Eigen::VectorXs v = mA * mX - mB; for (int i = 0; i < mX.size(); i++) { @@ -92,14 +94,65 @@ LCPSolutionType LCPUtils::getLCPSolutionType( const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices) { - Eigen::MatrixXs mANew = mA; - Eigen::VectorXs mXNew = mX; - Eigen::VectorXs mBNew = mB; - Eigen::VectorXs mHiNew = mHi; - Eigen::VectorXs mLoNew = mLo; - Eigen::VectorXi mFIndexNew = mFIndex; - ignoreFrictionIndices = !ignoreFrictionIndices; - return LCPSolutionType::FAILURE; + Eigen::VectorXs v = mA * mX - mB; + for (int i = 0; i < mX.size(); i++) + { + s_t upperLimit = mHi(i); + s_t lowerLimit = mLo(i); + if (mFIndex(i) != -1) + { + if (ignoreFrictionIndices) + { + if (mX(i) != 0) + return LCPSolutionType::FAILURE; + continue; + } + upperLimit *= mX(mFIndex(i)); + lowerLimit *= mX(mFIndex(i)); + } + + const s_t tol = 1e-5; + + /// Solves constriant impulses for a constrained group. The LCP formulation + /// setting that this function solve is A*x = b + w where each x[i], w[i] + /// satisfies one of + /// (1) x = lo, w >= 0 + /// (2) x = hi, w <= 0 + /// (3) lo < x < hi, w = 0 + + // If force has a zero bound, and we're at a zero bound (this is common with + // friction being upper-bounded by a near-zero normal force) then allow + // velocity in either direction. + if (abs(lowerLimit) < tol && abs(upperLimit) < tol && abs(mX(i)) < tol) + { + // This is always allowed + } + // If force is at the lower bound, velocity must be >= 0 + else if (abs(mX(i) - lowerLimit) < tol) + { + if (v(i) < -tol) + return LCPSolutionType::FAILURE; + } + // If force is at the upper bound, velocity must be <= 0 + else if (abs(mX(i) - upperLimit) < tol) + { + if (v(i) > tol) + return LCPSolutionType::FAILURE; + } + // If force is within bounds, then velocity must be zero + else if (mX(i) > lowerLimit && mX(i) < upperLimit) + { + if (abs(v(i)) > tol) + return LCPSolutionType::FAILURE; + } + // If force is out of bounds, we're always illegal + else + { + return LCPSolutionType::FAILURE; + } + } + // If we make it here, the solution is fine + return LCPSolutionType::SUCCESS; } //============================================================================== From 07032472cadb600d12752e02ae7586e298e66dde Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 23 Mar 2022 19:16:14 -0700 Subject: [PATCH 21/38] return boolean based on enum value --- dart/constraint/LCPUtils.cpp | 61 ++---------------------------------- 1 file changed, 2 insertions(+), 59 deletions(-) diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp index 90282868b..60227c129 100644 --- a/dart/constraint/LCPUtils.cpp +++ b/dart/constraint/LCPUtils.cpp @@ -21,65 +21,8 @@ bool LCPUtils::isLCPSolutionValid( { LCPSolutionType solutionType = getLCPSolutionType( mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); - (void)solutionType; // temporarily suppress unused variable warnings - Eigen::VectorXs v = mA * mX - mB; - for (int i = 0; i < mX.size(); i++) - { - s_t upperLimit = mHi(i); - s_t lowerLimit = mLo(i); - if (mFIndex(i) != -1) - { - if (ignoreFrictionIndices) - { - if (mX(i) != 0) - return false; - continue; - } - upperLimit *= mX(mFIndex(i)); - lowerLimit *= mX(mFIndex(i)); - } - - const s_t tol = 1e-5; - - /// Solves constriant impulses for a constrained group. The LCP formulation - /// setting that this function solve is A*x = b + w where each x[i], w[i] - /// satisfies one of - /// (1) x = lo, w >= 0 - /// (2) x = hi, w <= 0 - /// (3) lo < x < hi, w = 0 - - // If force has a zero bound, and we're at a zero bound (this is common with - // friction being upper-bounded by a near-zero normal force) then allow - // velocity in either direction. - if (abs(lowerLimit) < tol && abs(upperLimit) < tol && abs(mX(i)) < tol) - { - // This is always allowed - } - // If force is at the lower bound, velocity must be >= 0 - else if (abs(mX(i) - lowerLimit) < tol) - { - if (v(i) < -tol) - return false; - } - // If force is at the upper bound, velocity must be <= 0 - else if (abs(mX(i) - upperLimit) < tol) - { - if (v(i) > tol) - return false; - } - // If force is within bounds, then velocity must be zero - else if (mX(i) > lowerLimit && mX(i) < upperLimit) - { - if (abs(v(i)) > tol) - return false; - } - // If force is out of bounds, we're always illegal - else - { - return false; - } - } - // If we make it here, the solution is fine + if (solutionType == LCPSolutionType::FAILURE) + return false; return true; } From e18a7fcccc5e0d28936e05d0b875490665c899fa Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 23 Mar 2022 19:29:41 -0700 Subject: [PATCH 22/38] update solution types with different failure codes --- dart/constraint/LCPUtils.cpp | 16 ++++++++-------- dart/constraint/LCPUtils.hpp | 6 +++++- python/_nimblephysics/constraint/LCPUtils.cpp | 16 +++++++++++++++- 3 files changed, 28 insertions(+), 10 deletions(-) diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp index 60227c129..5e998c9af 100644 --- a/dart/constraint/LCPUtils.cpp +++ b/dart/constraint/LCPUtils.cpp @@ -21,9 +21,9 @@ bool LCPUtils::isLCPSolutionValid( { LCPSolutionType solutionType = getLCPSolutionType( mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); - if (solutionType == LCPSolutionType::FAILURE) - return false; - return true; + if (solutionType == LCPSolutionType::SUCCESS) + return true; + return false; } //============================================================================== @@ -47,7 +47,7 @@ LCPSolutionType LCPUtils::getLCPSolutionType( if (ignoreFrictionIndices) { if (mX(i) != 0) - return LCPSolutionType::FAILURE; + return LCPSolutionType::FAILURE_IGNORE_FRICTION; continue; } upperLimit *= mX(mFIndex(i)); @@ -74,24 +74,24 @@ LCPSolutionType LCPUtils::getLCPSolutionType( else if (abs(mX(i) - lowerLimit) < tol) { if (v(i) < -tol) - return LCPSolutionType::FAILURE; + return LCPSolutionType::FAILURE_LOWER_BOUND; } // If force is at the upper bound, velocity must be <= 0 else if (abs(mX(i) - upperLimit) < tol) { if (v(i) > tol) - return LCPSolutionType::FAILURE; + return LCPSolutionType::FAILURE_UPPER_BOUND; } // If force is within bounds, then velocity must be zero else if (mX(i) > lowerLimit && mX(i) < upperLimit) { if (abs(v(i)) > tol) - return LCPSolutionType::FAILURE; + return LCPSolutionType::FAILURE_WITHIN_BOUNDS; } // If force is out of bounds, we're always illegal else { - return LCPSolutionType::FAILURE; + return LCPSolutionType::FAILURE_OUT_OF_BOUNDS; } } // If we make it here, the solution is fine diff --git a/dart/constraint/LCPUtils.hpp b/dart/constraint/LCPUtils.hpp index 5927790fc..efeb2b821 100644 --- a/dart/constraint/LCPUtils.hpp +++ b/dart/constraint/LCPUtils.hpp @@ -13,7 +13,11 @@ namespace constraint { enum LCPSolutionType { SUCCESS, - FAILURE + FAILURE_IGNORE_FRICTION, + FAILURE_LOWER_BOUND, + FAILURE_UPPER_BOUND, + FAILURE_WITHIN_BOUNDS, + FAILURE_OUT_OF_BOUNDS }; class LCPUtils diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index 5c3505cdc..6a9d5fd6f 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -64,7 +64,21 @@ void LCPUtils(py::module& m) ::py::arg("ignoreFrictionIndices")); ::py::enum_(m, "LCPSolutionType") .value("SUCCESS", dart::constraint::LCPSolutionType::SUCCESS) - .value("FAILURE", dart::constraint::LCPSolutionType::FAILURE) + .value( + "FAILURE_IGNORE_FRICTION", + dart::constraint::LCPSolutionType::FAILURE_IGNORE_FRICTION) + .value( + "FAILURE_LOWER_BOUND", + dart::constraint::LCPSolutionType::FAILURE_LOWER_BOUND) + .value( + "FAILURE_UPPER_BOUND", + dart::constraint::LCPSolutionType::FAILURE_UPPER_BOUND) + .value( + "FAILURE_WITHIN_BOUNDS", + dart::constraint::LCPSolutionType::FAILURE_WITHIN_BOUNDS) + .value( + "FAILURE_OUT_OF_BOUNDS", + dart::constraint::LCPSolutionType::FAILURE_OUT_OF_BOUNDS) .export_values(); } From 342b0c9b84165ccf8eb5edbf432e77641ad4a83f Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Thu, 24 Mar 2022 08:48:35 -0700 Subject: [PATCH 23/38] return LcpResult in solveLcp --- dart/constraint/BoxedLcpConstraintSolver.cpp | 9 ++++++--- dart/constraint/BoxedLcpConstraintSolver.hpp | 8 +++++++- .../constraint/BoxedLcpConstraintSolver.cpp | 6 +++++- 3 files changed, 18 insertions(+), 5 deletions(-) diff --git a/dart/constraint/BoxedLcpConstraintSolver.cpp b/dart/constraint/BoxedLcpConstraintSolver.cpp index 6cd898a44..7a4ad989d 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.cpp +++ b/dart/constraint/BoxedLcpConstraintSolver.cpp @@ -349,7 +349,7 @@ LcpInputs BoxedLcpConstraintSolver::buildLcpInputs(ConstrainedGroup& group) } //============================================================================== -std::vector BoxedLcpConstraintSolver::solveLcp( +LcpResult BoxedLcpConstraintSolver::solveLcp( LcpInputs lcpInputs, ConstrainedGroup& group) { const std::size_t numConstraints = group.getNumConstraints(); @@ -785,7 +785,9 @@ std::vector BoxedLcpConstraintSolver::solveLcp( } constraintImpulses.push_back(mX.data() + mOffset[i]); } - return constraintImpulses; + LcpResult result; + result.impulses = constraintImpulses; + return result; } //============================================================================== @@ -793,7 +795,8 @@ std::vector BoxedLcpConstraintSolver::solveConstrainedGroup( ConstrainedGroup& group) { LcpInputs lcpInputs = buildLcpInputs(group); - return solveLcp(lcpInputs, group); + LcpResult result = solveLcp(lcpInputs, group); + return result.impulses; } //============================================================================== diff --git a/dart/constraint/BoxedLcpConstraintSolver.hpp b/dart/constraint/BoxedLcpConstraintSolver.hpp index 71d520d88..d1e67ff4b 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.hpp +++ b/dart/constraint/BoxedLcpConstraintSolver.hpp @@ -40,6 +40,12 @@ namespace dart { namespace constraint { +struct LcpResult +{ + // This is the solution to the LCP. + std::vector impulses; +}; + class BoxedLcpConstraintSolver : public ConstraintSolver { public: @@ -120,7 +126,7 @@ class BoxedLcpConstraintSolver : public ConstraintSolver LcpInputs buildLcpInputs(ConstrainedGroup& group); /// Setup and solve an LCP to enforce the constraints on the ConstrainedGroup. - std::vector solveLcp(LcpInputs lcpInputs, ConstrainedGroup& group); + LcpResult solveLcp(LcpInputs lcpInputs, ConstrainedGroup& group); protected: /// Boxed LCP solver diff --git a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp index 97b332266..42fc222bd 100644 --- a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp @@ -88,9 +88,13 @@ void BoxedLcpConstraintSolver(py::module& m) "solveLcp", +[](dart::constraint::BoxedLcpConstraintSolver* self, dart::constraint::LcpInputs lcpInputs, - dart::constraint::ConstrainedGroup& group) -> std::vector { + dart::constraint::ConstrainedGroup& group) + -> dart::constraint::LcpResult { return self->solveLcp(lcpInputs, group); }); + ::py::class_(m, "LcpResult") + .def(::py::init<>()) + .def_readwrite("impulses", &constraint::LcpResult::impulses); } } // namespace python From 0512d802343df2bb1e12f43662587769a0f2c287 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Thu, 24 Mar 2022 10:00:28 -0700 Subject: [PATCH 24/38] change impulses type to matrix --- dart/constraint/BoxedLcpConstraintSolver.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/dart/constraint/BoxedLcpConstraintSolver.hpp b/dart/constraint/BoxedLcpConstraintSolver.hpp index d1e67ff4b..7221d5717 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.hpp +++ b/dart/constraint/BoxedLcpConstraintSolver.hpp @@ -42,8 +42,9 @@ namespace constraint { struct LcpResult { - // This is the solution to the LCP. - std::vector impulses; + // This is the solution to the LCP, a matrix of impulses with shape + // (numContacts, 3). + Eigen::MatrixXs impulses; }; class BoxedLcpConstraintSolver : public ConstraintSolver From 7dc4e04dbfdd83a9dedaea295750b5b564d3a2a5 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Thu, 24 Mar 2022 22:43:36 -0700 Subject: [PATCH 25/38] refactor to check one constraint at a time --- dart/constraint/LCPUtils.cpp | 122 ++++++++++-------- dart/constraint/LCPUtils.hpp | 11 ++ python/_nimblephysics/constraint/LCPUtils.cpp | 11 ++ 3 files changed, 93 insertions(+), 51 deletions(-) diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp index 5e998c9af..fffc86061 100644 --- a/dart/constraint/LCPUtils.cpp +++ b/dart/constraint/LCPUtils.cpp @@ -19,7 +19,7 @@ bool LCPUtils::isLCPSolutionValid( const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices) { - LCPSolutionType solutionType = getLCPSolutionType( + LCPSolutionType solutionType = getLCPSolutionTypes( mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); if (solutionType == LCPSolutionType::SUCCESS) return true; @@ -27,8 +27,8 @@ bool LCPUtils::isLCPSolutionValid( } //============================================================================== -/// This checks whether a solution to an LCP problem is valid. -LCPSolutionType LCPUtils::getLCPSolutionType( +/// This determines the solution types of an LCP problem. +LCPSolutionType LCPUtils::getLCPSolutionTypes( const Eigen::MatrixXs& mA, const Eigen::VectorXs& mX, const Eigen::VectorXs& mB, @@ -40,59 +40,79 @@ LCPSolutionType LCPUtils::getLCPSolutionType( Eigen::VectorXs v = mA * mX - mB; for (int i = 0; i < mX.size(); i++) { - s_t upperLimit = mHi(i); - s_t lowerLimit = mLo(i); - if (mFIndex(i) != -1) + LCPSolutionType solType = getLCPSolutionType( + i, mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); + if (solType != LCPSolutionType::SUCCESS) + return solType; + } + return LCPSolutionType::SUCCESS; +} + +//============================================================================== +/// This determines the type of a solution to an LCP problem. +LCPSolutionType LCPUtils::getLCPSolutionType( + int i, + const Eigen::MatrixXs& mA, + const Eigen::VectorXs& mX, + const Eigen::VectorXs& mB, + const Eigen::VectorXs& mHi, + const Eigen::VectorXs& mLo, + const Eigen::VectorXi& mFIndex, + bool ignoreFrictionIndices) +{ + Eigen::VectorXs v = mA * mX - mB; + s_t upperLimit = mHi(i); + s_t lowerLimit = mLo(i); + if (mFIndex(i) != -1) + { + if (ignoreFrictionIndices) { - if (ignoreFrictionIndices) - { - if (mX(i) != 0) - return LCPSolutionType::FAILURE_IGNORE_FRICTION; - continue; - } - upperLimit *= mX(mFIndex(i)); - lowerLimit *= mX(mFIndex(i)); + if (mX(i) != 0) + return LCPSolutionType::FAILURE_IGNORE_FRICTION; + return LCPSolutionType::SUCCESS; } + upperLimit *= mX(mFIndex(i)); + lowerLimit *= mX(mFIndex(i)); + } - const s_t tol = 1e-5; + const s_t tol = 1e-5; - /// Solves constriant impulses for a constrained group. The LCP formulation - /// setting that this function solve is A*x = b + w where each x[i], w[i] - /// satisfies one of - /// (1) x = lo, w >= 0 - /// (2) x = hi, w <= 0 - /// (3) lo < x < hi, w = 0 + /// Solves constriant impulses for a constrained group. The LCP formulation + /// setting that this function solve is A*x = b + w where each x[i], w[i] + /// satisfies one of + /// (1) x = lo, w >= 0 + /// (2) x = hi, w <= 0 + /// (3) lo < x < hi, w = 0 - // If force has a zero bound, and we're at a zero bound (this is common with - // friction being upper-bounded by a near-zero normal force) then allow - // velocity in either direction. - if (abs(lowerLimit) < tol && abs(upperLimit) < tol && abs(mX(i)) < tol) - { - // This is always allowed - } - // If force is at the lower bound, velocity must be >= 0 - else if (abs(mX(i) - lowerLimit) < tol) - { - if (v(i) < -tol) - return LCPSolutionType::FAILURE_LOWER_BOUND; - } - // If force is at the upper bound, velocity must be <= 0 - else if (abs(mX(i) - upperLimit) < tol) - { - if (v(i) > tol) - return LCPSolutionType::FAILURE_UPPER_BOUND; - } - // If force is within bounds, then velocity must be zero - else if (mX(i) > lowerLimit && mX(i) < upperLimit) - { - if (abs(v(i)) > tol) - return LCPSolutionType::FAILURE_WITHIN_BOUNDS; - } - // If force is out of bounds, we're always illegal - else - { - return LCPSolutionType::FAILURE_OUT_OF_BOUNDS; - } + // If force has a zero bound, and we're at a zero bound (this is common with + // friction being upper-bounded by a near-zero normal force) then allow + // velocity in either direction. + if (abs(lowerLimit) < tol && abs(upperLimit) < tol && abs(mX(i)) < tol) + { + // This is always allowed + } + // If force is at the lower bound, velocity must be >= 0 + else if (abs(mX(i) - lowerLimit) < tol) + { + if (v(i) < -tol) + return LCPSolutionType::FAILURE_LOWER_BOUND; + } + // If force is at the upper bound, velocity must be <= 0 + else if (abs(mX(i) - upperLimit) < tol) + { + if (v(i) > tol) + return LCPSolutionType::FAILURE_UPPER_BOUND; + } + // If force is within bounds, then velocity must be zero + else if (mX(i) > lowerLimit && mX(i) < upperLimit) + { + if (abs(v(i)) > tol) + return LCPSolutionType::FAILURE_WITHIN_BOUNDS; + } + // If force is out of bounds, we're always illegal + else + { + return LCPSolutionType::FAILURE_OUT_OF_BOUNDS; } // If we make it here, the solution is fine return LCPSolutionType::SUCCESS; diff --git a/dart/constraint/LCPUtils.hpp b/dart/constraint/LCPUtils.hpp index efeb2b821..5fd8934e6 100644 --- a/dart/constraint/LCPUtils.hpp +++ b/dart/constraint/LCPUtils.hpp @@ -33,8 +33,19 @@ class LCPUtils const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices); + // This determines the solution types of an LCP problem. + static LCPSolutionType getLCPSolutionTypes( + const Eigen::MatrixXs& mA, + const Eigen::VectorXs& mX, + const Eigen::VectorXs& mB, + const Eigen::VectorXs& mHi, + const Eigen::VectorXs& mLo, + const Eigen::VectorXi& mFIndex, + bool ignoreFrictionIndices); + // This determines the type of a solution to an LCP problem. static LCPSolutionType getLCPSolutionType( + int i, const Eigen::MatrixXs& mA, const Eigen::VectorXs& mX, const Eigen::VectorXs& mB, diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index 6a9d5fd6f..4ea4fe1d6 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -52,9 +52,20 @@ void LCPUtils(py::module& m) ::py::arg("mLo"), ::py::arg("mFIndex"), ::py::arg("ignoreFrictionIndices")); + m.def( + "getLCPSolutionTypes", + &dart::constraint::LCPUtils::getLCPSolutionTypes, + ::py::arg("mA"), + ::py::arg("mX"), + ::py::arg("mB"), + ::py::arg("mHi"), + ::py::arg("mLo"), + ::py::arg("mFIndex"), + ::py::arg("ignoreFrictionIndices")); m.def( "getLCPSolutionType", &dart::constraint::LCPUtils::getLCPSolutionType, + ::py::arg("i"), ::py::arg("mA"), ::py::arg("mX"), ::py::arg("mB"), From bbafff1cbce35d96f4d7aecfc386fcbb49a3dc44 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Thu, 24 Mar 2022 22:55:33 -0700 Subject: [PATCH 26/38] return vector of solution types --- dart/constraint/LCPUtils.cpp | 27 +++++++++++++++++++-------- dart/constraint/LCPUtils.hpp | 2 +- 2 files changed, 20 insertions(+), 9 deletions(-) diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp index fffc86061..7da319c89 100644 --- a/dart/constraint/LCPUtils.cpp +++ b/dart/constraint/LCPUtils.cpp @@ -19,16 +19,21 @@ bool LCPUtils::isLCPSolutionValid( const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices) { - LCPSolutionType solutionType = getLCPSolutionTypes( + std::vector solutionTypes = getLCPSolutionTypes( mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); - if (solutionType == LCPSolutionType::SUCCESS) - return true; - return false; + + for (int i = 0; i < solutionTypes.size(); i++) + { + LCPSolutionType solutionType = solutionTypes[i]; + if (solutionType != LCPSolutionType::SUCCESS) + return false; + } + return true; } //============================================================================== /// This determines the solution types of an LCP problem. -LCPSolutionType LCPUtils::getLCPSolutionTypes( +std::vector LCPUtils::getLCPSolutionTypes( const Eigen::MatrixXs& mA, const Eigen::VectorXs& mX, const Eigen::VectorXs& mB, @@ -38,14 +43,20 @@ LCPSolutionType LCPUtils::getLCPSolutionTypes( bool ignoreFrictionIndices) { Eigen::VectorXs v = mA * mX - mB; + + std::vector solutionTypes; + for (int i = 0; i < mX.size(); i++) { LCPSolutionType solType = getLCPSolutionType( i, mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); - if (solType != LCPSolutionType::SUCCESS) - return solType; + solutionTypes.push_back(solType); + + // if (solType != LCPSolutionType::SUCCESS) + // return solType; } - return LCPSolutionType::SUCCESS; + return solutionTypes; + // return LCPSolutionType::SUCCESS; } //============================================================================== diff --git a/dart/constraint/LCPUtils.hpp b/dart/constraint/LCPUtils.hpp index 5fd8934e6..04da520ce 100644 --- a/dart/constraint/LCPUtils.hpp +++ b/dart/constraint/LCPUtils.hpp @@ -34,7 +34,7 @@ class LCPUtils bool ignoreFrictionIndices); // This determines the solution types of an LCP problem. - static LCPSolutionType getLCPSolutionTypes( + static std::vector getLCPSolutionTypes( const Eigen::MatrixXs& mA, const Eigen::VectorXs& mX, const Eigen::VectorXs& mB, From 8e14be22190fffba7c9a47c4f5ea882213c813ff Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Thu, 24 Mar 2022 23:03:56 -0700 Subject: [PATCH 27/38] include stlh --- python/_nimblephysics/constraint/LCPUtils.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index 4ea4fe1d6..07996ca89 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -34,6 +34,7 @@ #include #include #include +#include namespace py = pybind11; From 192284e03c0dbcc608a94bb3347a7f457bb02f2b Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Fri, 25 Mar 2022 07:38:47 -0700 Subject: [PATCH 28/38] add computeSlackFromLCPSolution() --- dart/constraint/LCPUtils.cpp | 16 +++++++++++----- dart/constraint/LCPUtils.hpp | 6 ++++++ python/_nimblephysics/constraint/LCPUtils.cpp | 6 ++++++ 3 files changed, 23 insertions(+), 5 deletions(-) diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp index 7da319c89..88f5b20ba 100644 --- a/dart/constraint/LCPUtils.cpp +++ b/dart/constraint/LCPUtils.cpp @@ -51,12 +51,8 @@ std::vector LCPUtils::getLCPSolutionTypes( LCPSolutionType solType = getLCPSolutionType( i, mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); solutionTypes.push_back(solType); - - // if (solType != LCPSolutionType::SUCCESS) - // return solType; } return solutionTypes; - // return LCPSolutionType::SUCCESS; } //============================================================================== @@ -71,7 +67,7 @@ LCPSolutionType LCPUtils::getLCPSolutionType( const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices) { - Eigen::VectorXs v = mA * mX - mB; + Eigen::VectorXs v = computeSlackFromLCPSolution(mA, mX, mB); s_t upperLimit = mHi(i); s_t lowerLimit = mLo(i); if (mFIndex(i) != -1) @@ -129,6 +125,16 @@ LCPSolutionType LCPUtils::getLCPSolutionType( return LCPSolutionType::SUCCESS; } +//============================================================================== +/// This computes the slack variable from the LCP solution. +Eigen::VectorXs LCPUtils::computeSlackFromLCPSolution( + const Eigen::MatrixXs& mA, + const Eigen::VectorXs& mX, + const Eigen::VectorXs& mB) +{ + return mA * mX - mB; +} + //============================================================================== /// This applies a simple algorithm to guess the solution to the LCP problem. /// It's not guaranteed to be correct, but it often can be if there is no diff --git a/dart/constraint/LCPUtils.hpp b/dart/constraint/LCPUtils.hpp index 04da520ce..1556993d4 100644 --- a/dart/constraint/LCPUtils.hpp +++ b/dart/constraint/LCPUtils.hpp @@ -54,6 +54,12 @@ class LCPUtils const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices); + // This computes the slack variable from the LCP solution. + static Eigen::VectorXs computeSlackFromLCPSolution( + const Eigen::MatrixXs& mA, + const Eigen::VectorXs& mX, + const Eigen::VectorXs& mB); + /// This applies a simple algorithm to guess the solution to the LCP problem. /// It's not guaranteed to be correct, but it often can be if there is no /// sliding friction on this timestep. diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index 07996ca89..fc5556557 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -74,6 +74,12 @@ void LCPUtils(py::module& m) ::py::arg("mLo"), ::py::arg("mFIndex"), ::py::arg("ignoreFrictionIndices")); + m.def( + "computeSlackFromLCPSolution", + &dart::constraint::LCPUtils::getLCPSolutionType, + ::py::arg("mA"), + ::py::arg("mX"), + ::py::arg("mB")); ::py::enum_(m, "LCPSolutionType") .value("SUCCESS", dart::constraint::LCPSolutionType::SUCCESS) .value( From 16e9de38518718f84ce73a0b1d8d3db79464a559 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Fri, 25 Mar 2022 07:41:05 -0700 Subject: [PATCH 29/38] fix typo --- python/_nimblephysics/constraint/LCPUtils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index fc5556557..c9d74be24 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -76,7 +76,7 @@ void LCPUtils(py::module& m) ::py::arg("ignoreFrictionIndices")); m.def( "computeSlackFromLCPSolution", - &dart::constraint::LCPUtils::getLCPSolutionType, + &dart::constraint::LCPUtils::computeSlackFromLCPSolution, ::py::arg("mA"), ::py::arg("mX"), ::py::arg("mB")); From ec9ba25387a7f33797af82f91293d1d60dfe9ce8 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Fri, 25 Mar 2022 20:57:58 -0700 Subject: [PATCH 30/38] add hadToIgnoreFrictionToSolve flag --- dart/constraint/BoxedLcpConstraintSolver.cpp | 1 + dart/constraint/BoxedLcpConstraintSolver.hpp | 3 +++ .../_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp | 5 ++++- 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/dart/constraint/BoxedLcpConstraintSolver.cpp b/dart/constraint/BoxedLcpConstraintSolver.cpp index 7a4ad989d..b2e4c014b 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.cpp +++ b/dart/constraint/BoxedLcpConstraintSolver.cpp @@ -787,6 +787,7 @@ LcpResult BoxedLcpConstraintSolver::solveLcp( } LcpResult result; result.impulses = constraintImpulses; + result.hadToIgnoreFrictionToSolve = hadToIgnoreFrictionToSolve; return result; } diff --git a/dart/constraint/BoxedLcpConstraintSolver.hpp b/dart/constraint/BoxedLcpConstraintSolver.hpp index 7221d5717..f604853c2 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.hpp +++ b/dart/constraint/BoxedLcpConstraintSolver.hpp @@ -45,6 +45,9 @@ struct LcpResult // This is the solution to the LCP, a matrix of impulses with shape // (numContacts, 3). Eigen::MatrixXs impulses; + + // Whether we fell back to solving a frictionless LCP. + bool hadToIgnoreFrictionToSolve; }; class BoxedLcpConstraintSolver : public ConstraintSolver diff --git a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp index 42fc222bd..c25c27589 100644 --- a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp @@ -94,7 +94,10 @@ void BoxedLcpConstraintSolver(py::module& m) }); ::py::class_(m, "LcpResult") .def(::py::init<>()) - .def_readwrite("impulses", &constraint::LcpResult::impulses); + .def_readwrite("impulses", &constraint::LcpResult::impulses) + .def_readwrite( + "hadToIgnoreFrictionToSolve", + &constraint::LcpResult::hadToIgnoreFrictionToSolve); } } // namespace python From 16aedc21ece46f7176afdd7c1fe3e216853d06c3 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Fri, 25 Mar 2022 21:29:32 -0700 Subject: [PATCH 31/38] add disableFrictionlessFallback flag --- dart/constraint/BoxedLcpConstraintSolver.cpp | 141 +++++++++--------- dart/constraint/BoxedLcpConstraintSolver.hpp | 5 +- .../constraint/BoxedLcpConstraintSolver.cpp | 12 +- 3 files changed, 85 insertions(+), 73 deletions(-) diff --git a/dart/constraint/BoxedLcpConstraintSolver.cpp b/dart/constraint/BoxedLcpConstraintSolver.cpp index b2e4c014b..1650d91dd 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.cpp +++ b/dart/constraint/BoxedLcpConstraintSolver.cpp @@ -350,7 +350,9 @@ LcpInputs BoxedLcpConstraintSolver::buildLcpInputs(ConstrainedGroup& group) //============================================================================== LcpResult BoxedLcpConstraintSolver::solveLcp( - LcpInputs lcpInputs, ConstrainedGroup& group) + LcpInputs lcpInputs, + ConstrainedGroup& group, + bool disableFrictionlessFallback) { const std::size_t numConstraints = group.getNumConstraints(); const std::size_t n = group.getTotalDimension(); @@ -603,77 +605,80 @@ LcpResult BoxedLcpConstraintSolver::solveLcp( // like the best we can do, given the limitations of boxed LCP solvers. In the // long run, we should probably reformulate the LCP problem to guarantee // solvability. - if (!success) + if (!disableFrictionlessFallback) { - hadToIgnoreFrictionToSolve = true; - - Eigen::MatrixXs mAReduced = mABackup.block(0, 0, n, n); - Eigen::VectorXs mXReduced = mXBackup; - Eigen::VectorXs mBReduced = mBBackup; - Eigen::VectorXs mHiReduced = mHiBackup; - Eigen::VectorXs mLoReduced = mLoBackup; - Eigen::VectorXi mFIndexReduced = mFIndexBackup; - Eigen::MatrixXs mapOut = LCPUtils::removeFriction( - mAReduced, - mXReduced, - mBReduced, - mHiReduced, - mLoReduced, - mFIndexReduced); - - mXReduced.setZero(); - - int reducedN = mXReduced.size(); - Eigen::Matrix - reducedAPadded = Eigen::MatrixXs::Zero(reducedN, dPAD(reducedN)); - reducedAPadded.block(0, 0, reducedN, reducedN) = mAReduced; - // Prefer using PGS to Dantzig at this point, if it's available - if (mSecondaryBoxedLcpSolver) - { - success = mSecondaryBoxedLcpSolver->solve( - reducedN, - reducedAPadded.data(), - mXReduced.data(), - mBReduced.data(), - 0, - mLoReduced.data(), - mHiReduced.data(), - mFIndexReduced.data(), - false); - } - else - { - success = mBoxedLcpSolver->solve( - reducedN, - reducedAPadded.data(), - mXReduced.data(), - mBReduced.data(), - 0, - mLoReduced.data(), - mHiReduced.data(), - mFIndexReduced.data(), - true); - } - mX = mapOut * mXReduced; - // Don't bother checking validity at this point, because we know the - // solution is invalid with friction constraints, and that's ok. - - /* -#ifndef NDEBUG - // If we still haven't succeeded, let's debug if (!success) { - std::cout << "Failed to solve LCP, even after disabling friction!" - << std::endl; - std::cout << "mAReduced: " << std::endl << mAReduced << std::endl; - std::cout << "mBReduced: " << std::endl << mBReduced << std::endl; - std::cout << "mFIndexReduced: " << std::endl - << mFIndexReduced << std::endl; - std::cout << "eigenvalues: " << std::endl - << mAReduced.eigenvalues() << std::endl; + hadToIgnoreFrictionToSolve = true; + + Eigen::MatrixXs mAReduced = mABackup.block(0, 0, n, n); + Eigen::VectorXs mXReduced = mXBackup; + Eigen::VectorXs mBReduced = mBBackup; + Eigen::VectorXs mHiReduced = mHiBackup; + Eigen::VectorXs mLoReduced = mLoBackup; + Eigen::VectorXi mFIndexReduced = mFIndexBackup; + Eigen::MatrixXs mapOut = LCPUtils::removeFriction( + mAReduced, + mXReduced, + mBReduced, + mHiReduced, + mLoReduced, + mFIndexReduced); + + mXReduced.setZero(); + + int reducedN = mXReduced.size(); + Eigen::Matrix + reducedAPadded = Eigen::MatrixXs::Zero(reducedN, dPAD(reducedN)); + reducedAPadded.block(0, 0, reducedN, reducedN) = mAReduced; + // Prefer using PGS to Dantzig at this point, if it's available + if (mSecondaryBoxedLcpSolver) + { + success = mSecondaryBoxedLcpSolver->solve( + reducedN, + reducedAPadded.data(), + mXReduced.data(), + mBReduced.data(), + 0, + mLoReduced.data(), + mHiReduced.data(), + mFIndexReduced.data(), + false); + } + else + { + success = mBoxedLcpSolver->solve( + reducedN, + reducedAPadded.data(), + mXReduced.data(), + mBReduced.data(), + 0, + mLoReduced.data(), + mHiReduced.data(), + mFIndexReduced.data(), + true); + } + mX = mapOut * mXReduced; + // Don't bother checking validity at this point, because we know the + // solution is invalid with friction constraints, and that's ok. + + /* + #ifndef NDEBUG + // If we still haven't succeeded, let's debug + if (!success) + { + std::cout << "Failed to solve LCP, even after disabling friction!" + << std::endl; + std::cout << "mAReduced: " << std::endl << mAReduced << std::endl; + std::cout << "mBReduced: " << std::endl << mBReduced << std::endl; + std::cout << "mFIndexReduced: " << std::endl + << mFIndexReduced << std::endl; + std::cout << "eigenvalues: " << std::endl + << mAReduced.eigenvalues() << std::endl; + } + #endif + */ } -#endif - */ } if (mX.hasNaN()) diff --git a/dart/constraint/BoxedLcpConstraintSolver.hpp b/dart/constraint/BoxedLcpConstraintSolver.hpp index f604853c2..09a741a04 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.hpp +++ b/dart/constraint/BoxedLcpConstraintSolver.hpp @@ -130,7 +130,10 @@ class BoxedLcpConstraintSolver : public ConstraintSolver LcpInputs buildLcpInputs(ConstrainedGroup& group); /// Setup and solve an LCP to enforce the constraints on the ConstrainedGroup. - LcpResult solveLcp(LcpInputs lcpInputs, ConstrainedGroup& group); + LcpResult solveLcp( + LcpInputs lcpInputs, + ConstrainedGroup& group, + bool disableFrictionlessFallback = false); protected: /// Boxed LCP solver diff --git a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp index c25c27589..5bb82a674 100644 --- a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp @@ -88,10 +88,14 @@ void BoxedLcpConstraintSolver(py::module& m) "solveLcp", +[](dart::constraint::BoxedLcpConstraintSolver* self, dart::constraint::LcpInputs lcpInputs, - dart::constraint::ConstrainedGroup& group) - -> dart::constraint::LcpResult { - return self->solveLcp(lcpInputs, group); - }); + dart::constraint::ConstrainedGroup& group, + bool disableFrictionlessFallback) -> dart::constraint::LcpResult { + return self->solveLcp( + lcpInputs, group, disableFrictionlessFallback); + }, + ::py::arg("lcpInputs"), + ::py::arg("group"), + ::py::arg("disableFrictionlessFallback") = false); ::py::class_(m, "LcpResult") .def(::py::init<>()) .def_readwrite("impulses", &constraint::LcpResult::impulses) From 1ef9ce57092a64c8c73435ffc6b53f4e6aa0d456 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Sat, 26 Mar 2022 10:12:43 -0700 Subject: [PATCH 32/38] add success attribute --- dart/constraint/BoxedLcpConstraintSolver.cpp | 1 + dart/constraint/BoxedLcpConstraintSolver.hpp | 3 +++ python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp | 1 + 3 files changed, 5 insertions(+) diff --git a/dart/constraint/BoxedLcpConstraintSolver.cpp b/dart/constraint/BoxedLcpConstraintSolver.cpp index 1650d91dd..3074dfd67 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.cpp +++ b/dart/constraint/BoxedLcpConstraintSolver.cpp @@ -791,6 +791,7 @@ LcpResult BoxedLcpConstraintSolver::solveLcp( constraintImpulses.push_back(mX.data() + mOffset[i]); } LcpResult result; + result.success = success; result.impulses = constraintImpulses; result.hadToIgnoreFrictionToSolve = hadToIgnoreFrictionToSolve; return result; diff --git a/dart/constraint/BoxedLcpConstraintSolver.hpp b/dart/constraint/BoxedLcpConstraintSolver.hpp index 09a741a04..82887ab91 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.hpp +++ b/dart/constraint/BoxedLcpConstraintSolver.hpp @@ -46,6 +46,9 @@ struct LcpResult // (numContacts, 3). Eigen::MatrixXs impulses; + // Whether Nimble says that it succeeded at solving the LCP. + bool success; + // Whether we fell back to solving a frictionless LCP. bool hadToIgnoreFrictionToSolve; }; diff --git a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp index 5bb82a674..9c5a89672 100644 --- a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp @@ -99,6 +99,7 @@ void BoxedLcpConstraintSolver(py::module& m) ::py::class_(m, "LcpResult") .def(::py::init<>()) .def_readwrite("impulses", &constraint::LcpResult::impulses) + .def_readwrite("success", &constraint::LcpResult::success) .def_readwrite( "hadToIgnoreFrictionToSolve", &constraint::LcpResult::hadToIgnoreFrictionToSolve); From 9dc2aeeebba6a2f8aec4f7839e83d6e09a97793d Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Mon, 28 Mar 2022 16:21:07 -0700 Subject: [PATCH 33/38] add python binding for removeShapeFramesOf(bodyNode) --- python/_nimblephysics/collision/CollisionGroup.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/python/_nimblephysics/collision/CollisionGroup.cpp b/python/_nimblephysics/collision/CollisionGroup.cpp index c64536420..e640eb483 100644 --- a/python/_nimblephysics/collision/CollisionGroup.cpp +++ b/python/_nimblephysics/collision/CollisionGroup.cpp @@ -95,6 +95,12 @@ void CollisionGroup(py::module& m) +[](dart::collision::CollisionGroup* self) { self->removeShapeFramesOf(); }) + .def( + "removeShapeFramesOf", + +[](dart::collision::CollisionGroup* self, + dart::dynamics::BodyNode* bodyNode) { + self->removeShapeFramesOf(bodyNode); + }) .def( "removeAllShapeFrames", +[](dart::collision::CollisionGroup* self) { From d14ec03c7a19b61c1449e651a733e4d48a85dd29 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Mon, 28 Mar 2022 16:30:47 -0700 Subject: [PATCH 34/38] add binding for addShapeFrames --- python/_nimblephysics/collision/CollisionGroup.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/python/_nimblephysics/collision/CollisionGroup.cpp b/python/_nimblephysics/collision/CollisionGroup.cpp index e640eb483..a00b0b288 100644 --- a/python/_nimblephysics/collision/CollisionGroup.cpp +++ b/python/_nimblephysics/collision/CollisionGroup.cpp @@ -74,6 +74,12 @@ void CollisionGroup(py::module& m) +[](dart::collision::CollisionGroup* self) { self->addShapeFramesOf(); }) + .def( + "addShapeFramesOf", + +[](dart::collision::CollisionGroup* self, + dart::dynamics::BodyNode* bodyNode) { + self->addShapeFramesOf(bodyNode); + }) .def( "subscribeTo", +[](dart::collision::CollisionGroup* self) { self->subscribeTo(); }) From 2c7c069d15feb778f09b7d1669812c0dba932958 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Fri, 15 Apr 2022 04:38:42 -0700 Subject: [PATCH 35/38] add getLocalVertices() --- python/_nimblephysics/dynamics/BodyNode.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/python/_nimblephysics/dynamics/BodyNode.cpp b/python/_nimblephysics/dynamics/BodyNode.cpp index 64c9f1383..2fa82e978 100644 --- a/python/_nimblephysics/dynamics/BodyNode.cpp +++ b/python/_nimblephysics/dynamics/BodyNode.cpp @@ -1007,6 +1007,12 @@ void BodyNode(py::module& m) .def( "removeAllShapeNodes", +[](dart::dynamics::BodyNode* self) { self->removeAllShapeNodes(); }) + .def( + "getLocalVertices", + +[](dart::dynamics::BodyNode* self) + -> const std::vector { + return self->getLocalVertices(); + }) .def( "getNumEndEffectors", +[](const dart::dynamics::BodyNode* self) -> std::size_t { From 17a2715ad8b461fae112b97b69e573488818b83b Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Fri, 15 Apr 2022 05:04:39 -0700 Subject: [PATCH 36/38] add MovingVertex --- python/_nimblephysics/dynamics/BodyNode.cpp | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/python/_nimblephysics/dynamics/BodyNode.cpp b/python/_nimblephysics/dynamics/BodyNode.cpp index 2fa82e978..a4ed685e5 100644 --- a/python/_nimblephysics/dynamics/BodyNode.cpp +++ b/python/_nimblephysics/dynamics/BodyNode.cpp @@ -171,6 +171,14 @@ void BodyNode(py::module& m) ::py::init(), ::py::arg("aspectProperties")); + ::py::class_(m, "MovingVertex") + .def(::py::init<>()) + .def_readwrite("pos", &dart::dynamics::BodyNode::MovingVertex::pos) + .def_readwrite("vel", &dart::dynamics::BodyNode::MovingVertex::vel) + .def_readwrite("accel", &dart::dynamics::BodyNode::MovingVertex::accel) + .def_readwrite( + "timestep", &dart::dynamics::BodyNode::MovingVertex::timestep); + ::py::class_< dart::dynamics::TemplatedJacobianNode, dart::dynamics::JacobianNode, @@ -1009,10 +1017,17 @@ void BodyNode(py::module& m) +[](dart::dynamics::BodyNode* self) { self->removeAllShapeNodes(); }) .def( "getLocalVertices", - +[](dart::dynamics::BodyNode* self) + +[](const dart::dynamics::BodyNode* self) -> const std::vector { return self->getLocalVertices(); }) + .def( + "getMovingVerticesInWorldSpace", + +[](const dart::dynamics::BodyNode* self, int timestep) + -> std::vector { + return self->getMovingVerticesInWorldSpace(timestep); + }, + ::py::arg("timestep") = -1) .def( "getNumEndEffectors", +[](const dart::dynamics::BodyNode* self) -> std::size_t { From 507d326857431c35a0b07d365d2fe1e09d18a51a Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Fri, 15 Apr 2022 05:04:58 -0700 Subject: [PATCH 37/38] add body node --- python/_nimblephysics/dynamics/BodyNode.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python/_nimblephysics/dynamics/BodyNode.cpp b/python/_nimblephysics/dynamics/BodyNode.cpp index a4ed685e5..564725af2 100644 --- a/python/_nimblephysics/dynamics/BodyNode.cpp +++ b/python/_nimblephysics/dynamics/BodyNode.cpp @@ -176,6 +176,8 @@ void BodyNode(py::module& m) .def_readwrite("pos", &dart::dynamics::BodyNode::MovingVertex::pos) .def_readwrite("vel", &dart::dynamics::BodyNode::MovingVertex::vel) .def_readwrite("accel", &dart::dynamics::BodyNode::MovingVertex::accel) + .def_readwrite( + "bodyNode", &dart::dynamics::BodyNode::MovingVertex::bodyNode) .def_readwrite( "timestep", &dart::dynamics::BodyNode::MovingVertex::timestep); From 61ab8b24f54007bbe7b6e4fd6c8588e6317a5db9 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Fri, 15 Apr 2022 05:17:23 -0700 Subject: [PATCH 38/38] add constructor args --- python/_nimblephysics/dynamics/BodyNode.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/python/_nimblephysics/dynamics/BodyNode.cpp b/python/_nimblephysics/dynamics/BodyNode.cpp index 564725af2..c6d322ab3 100644 --- a/python/_nimblephysics/dynamics/BodyNode.cpp +++ b/python/_nimblephysics/dynamics/BodyNode.cpp @@ -172,7 +172,18 @@ void BodyNode(py::module& m) ::py::arg("aspectProperties")); ::py::class_(m, "MovingVertex") - .def(::py::init<>()) + .def( + ::py::init< + Eigen::Vector3s, + Eigen::Vector3s, + Eigen::Vector3s, + dart::dynamics::BodyNode*, + int>(), + ::py::arg("pos"), + ::py::arg("vel"), + ::py::arg("accel"), + ::py::arg("bodyNode"), + ::py::arg("timestep")) .def_readwrite("pos", &dart::dynamics::BodyNode::MovingVertex::pos) .def_readwrite("vel", &dart::dynamics::BodyNode::MovingVertex::vel) .def_readwrite("accel", &dart::dynamics::BodyNode::MovingVertex::accel)